Short Week with the Gyro

Once again a big congratulations to FIRST Team 3182, Athena's Warriors for an excellent performance at the Hartford FIRST Robotics Competition! As I'm sure everyone experienced for themselves the past few days, you have a lot to be proud of. Excellent job team! I'm looking forward to next season!

The Gyro

Even though I was occupied at the end of the week with the competition, I was still able to get some work done investigating the gyro thanks to HiTechnic and their very prompt delivery! It's handy to be close to the distribution center! :)

The HiTechnic gyroscopic sensor is a piezoelectric device that is capable of measuring the angular velocity of the sensor in a single plane. By mounting the sensor on the robot in an appropriate orientation, we will be able to replace the accelerometer as a means of detecting the tilt of the robot.

I've modified the original design to mount both sensors for now, with the gyro on the right side of the image (left side of the robot). I've also added some larger wheels for improved velocity at the base. Additional mass these components add is negligible from my earlier measurements. Here's the Mark II platform (Mark I is from 2009).

Mark II platform

Gyro Initialization

Before any reading can be done for the controller, we must first initialize the gyroscopic sensor with an offset value. The following code finds a offset value automatically based on readings from the sensor in a stable location. This code is based on the HTWay's gyro initialization.

// setup some temporary variables
int gyroMin_degPs,
    gyroMax_degPs,
    gyroRaw_degPs,
    gyroSum_degPs;

int it = 0;

float g_gyroOffset_degPs;

SetSensorHTGyro(S4);
Wait(100);

// find the initial gyro offset
do {
  gyroSum_degPs = 0;
  gyroMin_degPs = 1000;
  gyroMax_degPs = -1000;
  for (it = 0; it < 100; it++) {
    gyroRaw_degPs = SensorHTGyro(S4);
    if (gyroRaw_degPs > gyroMax_degPs) {gyroMax_degPs = gyroRaw_degPs;}
    if (gyroRaw_degPs < gyroMin_degPs) {gyroMin_degPs = gyroRaw_degPs;}
    gyroSum_degPs += gyroRaw_degPs;
    Wait(5);
  }
} while ((gyroMax_degPs - gyroMin_degPs) > 1);   // Reject and sample again if range too large

// average the sum of the samples
g_gyroOffset_degPs = gyroSum_degPs / 100.0;

After setting some initial variables, the SetSentorHTGyro() call sets the sensor to be on port 4 of the NXT. The Wait() immediately following it makes sure that the gyro is settled and will not send out garbage data immediately after initializing it. The embedded loops that follow read a raw value from the sensor, update some minimum and maximum values, and sum the raw readings together. This is repeated 100 times. If the minimum and maximum values are too far apart, that process is repeated and a new set of 100 data points are sampled. This guarantees the sensor wasn't moved during the initialization process. If the minimum and maximum values are close enough, the sum is averaged to find the sampled gyro offset.

Gyro Reading

To get an idea of what the drift on the gyro looks like, I put together the following code. This code will prepare a file with 513 records of tick times (milliseconds) and raw readings. While leaving the sensor untouched, any change in the sensor's value is caused by the natural drift.

This code is based on the HTWay's gyro reading code. Note that the offset I determined with the code above isn't used here since I'm most interested in the natural drift of the sensor, not necessarily its drift from a sample.

// setup some temporary variables
int firstTick, lastTick;
int t1, t2, t3, t4;
int i, bytesWritten, tmp;
byte fileHandle;
string buffer;

DeleteFile("gyro_idle.csv");
CreateFile("gyro_idle.csv", 4096*2, fileHandle);

firstTick = CurrentTick();
lastTick = firstTick;
for (i = 0; i <= 512; i++) {
  // read the sensor
  tmp = CurrentTick() - firstTick;
  buffer = StrCat(NumToStr(tmp), ",");
  t1 = CurrentTick();
  tmp = SensorHTGyro(S4);
  buffer = StrCat(buffer, NumToStr(tmp));
  t2 = CurrentTick();
  WriteLnString(fileHandle, buffer, bytesWritten);
  t3 = CurrentTick();
  
  // output timing debug info
  ClearScreen();
  NumOut(10, LCD_LINE1, t1 - lastTick);
  NumOut(10, LCD_LINE2, t2 - t1);
  NumOut(10, LCD_LINE3, t3 - t2);
  t4 = CurrentTick();
  NumOut(10, LCD_LINE4, t4 - t3);
   
  lastTick = CurrentTick();
  Wait((20));
}

CloseFile(fileHandle);

This code begins with initializing temporary variables. Next, a file is deleted and created in preparation for the records to be written to it. After calculating an initial tick value, the main loop is run. For each iteration, a string of the current tick and the current gyro value are concatenated separated by a comma. The resulting string is written to the file. The additional lines dealing with timing are printed to the NXT's LCD screen and used to debug the loop timing. Finally, the loop waits 20 ticks before running again. At its completion, the file is closed for good house keeping.

The Results

The resulting data file can be acquired by using a utility from the BricxCC's NXT Explorer. Without even plotting this data, it's pretty obvious that there are more readings at a -1 value at the start of the test then at the end, but for good measure, here's a plot of the same information.

Raw gyro values at idle

Without a doubt, this drift will need to be accounted for.