Penalizing mecanum wheeled robots durring alliance selection.

[Chris Hibner]

So, how do you actually do it?

First, we throw out the WPI gyro library and just use the analog input library. You'll need to know the sensitivity of your gyro (in deg/s/V), but if you don't know that you can make a guess and figure it out with some testing. We always do the tests anyway to characterize the sensor we're using.

You need a few things in your code:

1) An array of gyro voltage readings so you can calculate a moving average. This moving average will be stopped once the match begins and this average will become your gyro bias.

2) A variable to indicate that your robot has become enabled once during this power cycle. Why is this important? Because you don't want to resume calculating the bias in that short disabled period between auto and teleop. You may say, "but you said a bias calculated as close to the the current time possible is better, so wouldn't it be good to calculate it again just before teleop?" The answer is no, because then you have a good chance of violating the most important criteria: the robot MUST be still. There is a decent chance that either your robot was still in motion as power was cut to end autonomous, or some other robot runs into you at the end of autonomous. Either way, the safe bet is to just use the bias from just before auto mode starts.

3) An integral to calculate the heading from the angular rate samples. (If you don't know what that is, it's a fancy term for area under a curve, which is a big sum. Look it up on wikipedia.)

So here it goes with the code:

(note: caveats apply. a) I'm doing this by the seat of my pants, so there may be a bug or two; b) this is more intended to be pseudo-code rather than copy-and-paste C code; c) this is intended to provide a general idea - not a final solution - see (a) and (b))

(note 2: I'm going to assume you have a typedef.h file and use descriptive types. f32 = 32-bit float, u8 = unsigned 8-bit integer, s16 = signed 16-bit integer, etc.)

defines and global variables

#define GYRO_BIAS_SIZE 32
#define GYRO_ANALOG_CHAN 1
#define GYRO_SENSITIVITY 0.025 /* in deg/s/V */

f64 gyroBiasArray[GYRO_BIAS_SIZE];
f64 gyroBiasSum;
u8 gyroBiasIdx;
bool visitedEnabled;
f64 gyroHeading;
f64 currentTime;
f64 previousTime;

In your "init" function

gyroBiasSum = 0;
gyroBiasIdx = 0;
visitedEnabled = FALSE;
gyroHeading = 0;

for (u8 i = 0; i < GYRO_BIAS_SIZE; i++)
{
   gyroBiasArray[i] = 0;
}

In your fastest periodic loop

f64 gyroV;
f64 gyroDPS;   /* in deg/sec */
static f64 gyroDPS_prev = 0;  /* deg/sec previous sample - used for trapezoidal integration */

if ((getFMSMode() == AUTO) || (getFMSMode() == TELEOP))
{
    visitedEnabled = TRUE;
}

previousTime = currentTime;
currentTime = getTickCnt_ms() / 1000;

/* sample the gyro voltage */
gyroV = getAnalogVoltage(GYRO_ANALOG_CHAN);


/* calculate the bias if we have not yet been enabled */
if (FALSE == visitedEnabled)
{
    /* compute the moving average by first calculating a moving sum
       the moving sum is computed by first subtracting the oldest
       sample in the bias array, then add the current sample.  Then
       replace the oldest sample in the bias array with the newest sample. */
    gyroBiasSum -= gyroBiasArray[gyroBiasIdx];
    gyroBiasSum += gyroV;
    gyroBiasArray[gyroBiasIdx] = gyroV;
    gyroBiasIdx++;
    if (gyroBiasIdx >= GYRO_BIAS_SIZE)
    {
        gyroBiasIdx = 0;
    }
}
else
{
    /* if we're here, it means we've been enabled.  Therefore, we need to
       be calculating the heading instead of the bias. */

    /* first, subtract bias voltage and convert to degrees/sec */
    gyroDPS = (gyroV - (gyroBiasSum / GYRO_BIAS_SIZE)) * GYRO_SENSITIVITY;
    /* Now, compute heading using trapezoidal integration */
    gyroHeading += ((gyroDPS + gyroDPS_prev)/2) * (currentTime - previousTime);
    gyroDPS_prev = gyroDPS;
}

And there you have it.

I would highly recommend characterizing the sensor you're using rather than just using the sensitivity from the data sheet. Not only can the sensitivity vary from part to part, but the sensitivity can be reduced if the gyro isn't mounted perfectly flat. Here's how we do it:

1) make your best guess at the gyro sensitivity (use the nominal sensitivity in the data sheet if you have it).

2) Place the robot flat up against a wall and start your code.

3) After sufficient time to allow a good bias calculation, enable your robot into teleop mode.

4) Rotate your robot 180 degrees and place the robot flat against the wall again (so you ensure your robot rotated as close to exactly 180 degrees as possible).

5) Write down the computed heading.

6) adjust your gyro sensitivity in your code using the following calculation: NewGyroSensitivity = OldGyroSensitivity * ActualRecordedHeading / 180

7) Repeat the procedure until your recorded heading is VERY close to 180 deg.

Last note: we actually use LabVIEW. I can post that as well if you don't do textual programming.

If there are any questions, let me know.