Gyro calibration in RobotC?


#1

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);
    }
}

}

IMG_2063.JPG


#2

Chris,

The one thing our team fond that worked while trying to make gyro controlled turns and moves was this:

The kids programmed our bot to turn for a certain amount of time (setting the motor speed based on a formula that used the gyro’s rotation reading (higher speed when farther from the goal, lower spewed as it got closer). This way if a wild value was reported by the gyro it didn’t stop the turn early.

Then after the that timed turn was done, the program looked at the gyro value to adjust the right and left motor speeds to straighten up the next forward move, that way if the turn ended too soon or too late it would correct the direction of travel in the first part of the next straight forward move.

Hope this helps.


#3

After re-reading your original post, I have a couple more bits of advice:

The over turning may be due to momentum. If the motors turn off right when the gyro reaches 90 degrees, the momentum of the moving motor and gears can carry the robot past 90 degrees. If you set holding to “on” for the drive motors this may fix some of that (not sure how to do that in RobotC we use Modkit, and maybe that has already been done).

We found that there was enough slop in the gears and inconsistency with the floor (field) that there was a lot of variability in the result of the gyro controlled turns and thus the previously posted solution.


#4

Overshooting a turn typically occurs when the robot is turning fast. It could be the inertia of the robot, or it could be the speed at which the gyro obtains its new value. Our robots typically have omni wheels and are turning fast, so I thought it was inertia. In your first code sample the motor speed is only 30, so I would expect the robot to be moving slowly, unless you have a high gear ratio. What is your gear ratio?

For diagnostics, I would suggest displaying the current (or last) gyro reading on the screen.

Here are some things to try:

Dial the speed down even slower on the turns.

Put a delay of one or two seconds after the turn is complete and before giving the robot the next motor command. It would also be interesting to display the gyro value before and after the wait.

You could also try changing the brake mode to ‘Hold’ [setMotorBrakeMode(mtrRight,motorHold)] (this may or may not be the same as Hold=On in Modkit, in Robot C there are three modes Coast, Brake (default), and Hold). We have noticed that when using this setting for robots with omni wheels, that it causes a rocking movement at the end of the turn (the kids call it the ‘Jello’ setting). You may need to add a wait to the end of your turn to give the robot time to settle down if you use this setting.


#5

In Both cases the gear ratios are 1:1, so they’re not going TERRIBLY fast (far less than 250 degrees/sec). We’ll do some display settings to see what else we can get. Do you find that you need to wait after resetting the gyro, but before moving? My assumption was that resetting the gyro just resetting an internal variable value. We will also try hold mode.


#6

We don’t reset the gyros often, but that’s just us. In our autonomous routines there is usually a point where someone manually moves the robot back to the starting square. That is the only place where we have used the reset.


#7

Any solutions to this issue? We have just purchased a brand new vex iq kit and a gyro sensor, we are using RobotC and the sensor just keeps decrementing. I see in RobotC there are some parameters for calibrating the gyro e.g. “gyroCalibrateSamples1024” but there is no documentation I can find or sample code I can find that can help me calibrate it. I tried lowering the sensitivity… all that does is slow down the rate at which the sensor decrements.


#8

We’ve always managed very accurate 90 degree turns using the gyro but making sure the robot decelerates towards 90 degrees. In it’s absolute simplest form, something like:


while(getGyroDegrees(gyro) < 90) {
  if(getGyroDegrees(gyro) < 70) {
    setMotor(rightMotor, 80);
    setMotor(rightMotor, -80);
  }
  else {
     setMotor(rightMotor, 20);
     setMotor(rightMotor, -20);
  }
}
setMotor(rightMotor, 0);
setMotor(rightMotor, 0);


#9

jpearman - no we do not have anything like that, that’s what I was looking for! Thank you. I looked high and low for documentation and couldn’t find anything. Maybe I missed it. I tried looking in the instructions in the IDE itself, the online RobotC help file and so on and didn’t find anything. I tried typing random stuff in the IDE to see if autocomplete would give me some hints…that helped me find the gyroCalibrateSamplesXXXX parameters, but I never found the startcalibrate. Thank You!

calvc01 - yes we have some similar code that slows down to get more precise turns, very similar to yours. Thank you for the input!


#10

jpearman, well, I implement this code and it seems as though 9 out of 10 times the getGyroCalibrationFlag stays high and this thing never calibrates. So the 1 out of 10 times, it calibrates and the former decrementing doesn’t happen…and of course on the 9 out of 10… decrementing issue is still present. I will say on the 1 out of 10 where the calibrate flag does go low and lets it out of the loop, it does increment the angle but at a very low acceptable rate, say 1 degree in a minute. Any ideas on making this sensor consistently calibrate? or do I have a bad sensor?


#11

With no calibration, the sensor is what I would call “unusable”…within 60 seconds you have 60 degrees of error. There’s no way a student can get a functional program that way. Last night, it did seem to calibrate more consistently, not by really doing anything code or hardware wise…it just started calibrating successfully more often “on its on”.

The firmware is up to date.

I did notice that with the calibration it loses (on occasion gains) one (1) degree between 30 seconds to a little over a minute (varies depending on the calibration). I calibrated it several times and timed it between increments/decrements on the gyro for at least 4 gained or lost degrees each time. So even after cal it loses/gains position, albeit takes 30-60 times longer.

I’ll reach out to Vex or whoever today and see if they think this is a bad sensor, see if I can get a replacement.

EDIT: to clarify, by gained or lost I mean that when it cals it will gain or lose in the same direction until I do another call e.g. it will decrement to -4 within 2 to 4 minutes OR sometimes it will gain position and increment to 4 within 2 to 4 minutes.
code..jpg


#12

I found a solution that works for my VEX IQ. see test code below.
#pragma config(Sensor, port5, gyroSensor, sensorVexIQ_Gyro)

// gyro sensor test - UNDOCUMENTED calibration is needed to slow down the reading drift,
// otherwise drift +1 deg/sec !!!
// http://www.vexiqforum.com/forum/main-category/technical-discussion/32797-gyro-sensor-drift

//Function to calibrate gyro
void iqCalibrateGyro() {
// startGyroCalibration(gyroSensor, gyroCalibrateSamples512); // short cal, 512pt
startGyroCalibration(gyroSensor, gyroCalibrateSamples1024); // longer cal, works better
// delay so calibrate flag can be set internally to the gyro
wait1Msec(100);
displayTextLine(1, “Calibrating…”);
while (getGyroCalibrationFlag(gyroSensor)) {}; // wait for cal to complete
resetGyro(gyroSensor); // reset to 0 heading
}

task main()
{
//Reset the gyro sensor to remove any previous data.
//resetGyro(gyroSensor);
iqCalibrateGyro();

while(true) {
//Display the 3 different values available from the Gyro Sensor
displayTextLine(1, “Degrees: %d”, getGyroDegrees(gyroSensor));
displayTextLine(2, “Heading: %d”, getGyroHeading(gyroSensor));
displayTextLine(3, " Rate: %d", getGyroRate(gyroSensor));
sleep(50); //Wait for 50ms to allow new data to be aquired.
}
}