Hey there,
I’m teaching an intro to engineering course and using VexIQ and RobotC. I have given my students a task that involves navigating a course using distance, color, and gyro sensors. It uses a non-linear state machine to traverse the course (see image below, the red box is an old arena for another activity).
The students are consistently having issues with the gyro not properly detecting a 90 degree turn. We have several iterations of the code, but they always seem to overshoot the target angle by about 10 degrees. We have tried accumulator variables, altering the sensitivity, changing how the states execute, and more. Below the image I’ve attached two students’ code sequences.
If there is something we’re missing in trying to get the gyro to correctly read 90 degrees, and if anyone could spot that issue, I would greatly appreciate it.
Thanks,
Chris
Code sample 1 (note the degrees have already been changed to try to compensate for the overshooting)
#pragma config(Sensor, port4, huey, sensorVexIQ_ColorHue)
#pragma config(Sensor, port5, turny, sensorVexIQ_Gyro)
#pragma config(Sensor, port10, echo, sensorVexIQ_Distance)
#pragma config(Sensor, port11, touchy, sensorVexIQ_LED)
#pragma config(Motor, motor6, right, tmotorVexIQ, PIDControl, driveRight, encoder)
#pragma config(Motor, motor12, left, tmotorVexIQ, PIDControl, reversed, driveLeft, encoder)
//!!Code automatically generated by ‘ROBOTC’ configuration wizard !!//
task main()
{
int state = 0;
int hue = 0;
float degrees = 0;
bool running = false;
clearTimer(T1);
resetGyro(turny);
setTouchLEDColor(touchy, colorRed);
setGyroSensitivity(turny, gyroHighSensitivity);
while (true)
{
if (getTouchLEDValue(touchy) == 1 && running == false) //If it is touched and you are not running the routine
{
state = 1; //reset state to first step
running = true;
}
if(!running)
setTouchLEDColor(touchy, colorRed);
if(running == true)
{
switch(state)
{
case 1: //Set color to Yellow drive forward
setTouchLEDColor(touchy, colorYellow);
setMotorSpeed(left, 30);
setMotorSpeed(right, 30);
if(getDistanceValue(echo) <= 177.8)
{
state++;
setMotorSpeed(left, 0);
setMotorSpeed(right, 0);
resetGyro(turny);
degrees = getGyroDegreesFloat(turny);
}
break;
case 2: //detect Color
hue = getColorHue(huey);
if (hue >= 75 && hue <= 135) //green
state = 3;
else if (hue >= 140 && hue <= 180) // blue
state = 4;
else if (hue <= 25 || hue >= 130) // red
state = 5;
break;
case 3: //Set color to Green and turn cw
setTouchLEDColor(touchy, colorGreen);
setMotorSpeed(left, 35);
setMotorSpeed(right, -35);
if(getGyroDegreesFloat(turny) - degrees <= -79)
{
setMotorSpeed(left, 0);
setMotorSpeed(right, 0);
state = 1;
resetGyro(turny);
degrees = getGyroDegreesFloat(turny);
}
break;
case 4: //Set color to Blue and turn ccw
setTouchLEDColor(touchy, colorBlue);
setMotorSpeed(left, -35);
setMotorSpeed(right, 35);
if(getGyroDegreesFloat(turny) - degrees >= 76)
{
setMotorSpeed(left, 0);
setMotorSpeed(right, 0);
state = 1;
resetGyro(turny);
degrees = getGyroDegreesFloat(turny);
}
break;
case 5: //Set color to Red and stop
setTouchLEDColor(touchy, colorRed);
setMotorSpeed(left, 0);
setMotorSpeed(right, 0);
running = false;
}
}
}
}
Sample 2, using a loop inside of the case to eliminate excess logic – still overshoots:
#pragma config(Sensor, port2, distSonar, sensorVexIQ_Distance)
#pragma config(Sensor, port6, tchLED, sensorVexIQ_LED)
#pragma config(Sensor, port8, colorScan, sensorVexIQ_ColorHue)
#pragma config(Sensor, port12, gyroSnr, sensorVexIQ_Gyro)
#pragma config(Motor, motor1, mtrRight, tmotorVexIQ, PIDControl, reversed, encoder)
#pragma config(Motor, motor7, mtrLeft, tmotorVexIQ, PIDControl, encoder)
//* BUT THAT CAMEL NOTATION THO //
// #autoformattingswag //
// GOTTA GET THAT AUTON DONE BRUH *//
task main()
{
int state = 1;
bool run = false;
setTouchLEDColor(tchLED, colorRed);
setGyroSensitivity(gyroSnr,gyroHighSensitivity);
resetGyro(gyroSnr);
while (true)
{
setColorMode(colorScan, colorTypeRGB_Hue_Ambient);
if(run == true)
{
switch(state)
{
case 1:
setTouchLEDColor(tchLED, colorGreen);
setMotorSpeed(mtrRight, 80);
setMotorSpeed(mtrLeft, 80);
displayTextLine(1,"Navigating...");
if(getDistanceValue(distSonar) / 25.4 <= 7)
{
state++;
clearTimer(T1);
setMotorSpeed(mtrRight, 0);
setMotorSpeed(mtrLeft, 0);
resetGyro(gyroSnr);
}
break;
case 2:
if(getColorHue(colorScan) >= 75 && getColorHue(colorScan) <= 135)
{
state = 3;
}
else if(getColorHue(colorScan) >= 140 && getColorHue(colorScan) <= 180)
{
state = 4;
}
else if(getColorHue(colorScan) >= 230 && getColorHue(colorScan) <= 25)
{
state = 5;
}
break;
case 3:
resetGyro(gyroSnr);
while(getGyroDegreesFloat(gyroSnr) < 78.35)
{
setMotorSpeed(mtrRight, -80);
setMotorSpeed(mtrLeft, 80);
}
setMotorSpeed(mtrRight, 0);
setMotorSpeed(mtrLeft, 0);
state = 1;
break;
case 4:
resetGyro(gyroSnr);
while(getGyroDegreesFloat(gyroSnr) > -78.35)
{
setMotorSpeed(mtrRight, 80);
setMotorSpeed(mtrLeft, -80);
}
setMotorSpeed(mtrRight, 0);
setMotorSpeed(mtrLeft, 0);
state = 1;
break;
case 5:
setTouchLEDColor(tchLED, colorRed);
setMotorSpeed(mtrRight, 0);
setMotorSpeed(mtrLeft, 0);
displayTextLine(1,"Maze navigated");
displayTextLine(2,"Omega good job");
run = false;
break;
}
}
else if (getTouchLEDValue(tchLED) == 1 && run == false)
{
state = 1;
run = true;
resetGyro(gyroSnr);
}
}
}
