Code: #pragma config(Sensor, in3, gyro, sensorGyro) //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // // Gyro Driver Test // // This program emulates the gyro value calculation of ROBOTC's internal driver. It only emulates the value calculation. // It does not emulate the calculation of the bias values and relies upon the value calculated by the internal driver. // // When gyro driver starts up, it first calculates the internal bias value. The bias value is the analog value output // by the gyroscope while it is perfectly still. The steps to calculate the bias. // // 1. Delay for 200 milliseconds. This is to allow the gyro to stabilize when it is first powered up. The datasheet // indicates that this may take 50 milliseconds so we'll run it a little longer. // // 2. Measure the analog value every 0.001 seconds and calculate the sum over 1024 samples. // // 3. The bias can be calculated. Take the sum from (2) and divide by 1024. // // 4. Normally the remainder from (3) is non-zero. The remainder term is called the "small bias" by ROBOTC firmware. // // 5. Driver is now ready to calculate the gyro value. // // The gyro does not output an analog value representing angular position. Instead the value represents angular velocity. // To calculate angular position, you have to integrate periodically measured samples. The internal ROBOTC driver does // this is follows: // // 1. Every millisecond it sums the "difference" of the current analog gyro value with the calculated bias value. This // effectively integrates the gyro velocity values into a gyro rotation value. // // 2. In (4) above there was a residual error (i.e. the small bias) calculated. So on every 1024 sums, the 'small bias' // is subtracted from the accumulated sum. // // 3. When user application program requests the gyro value, the accumulated sum is divided by the 'SensorScale' setting // to scale the result in tenths of a degree. // // 4. The analog gyro value has a lot of jitter; i.e. if the bias value is 1823 then the analog readings on a non-moving // gyro may be 1820 to 1826. The jitter does not appear to be uniformly distributed around the bias value. So the driver // has a "jitter filter" to ignore analog values that are within '4' (see '#define' for 'kJitterIgnoreRange') of the // bias value. This eliminates the // // 5. Even with point (4) above, there is a small drift in the gyro. This is typical of these gyro ICs. Over several minutes // the gyro value range -15 to +15 degrees (i.e. value of 150 in tenths of a degree) . // ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// long nNumbOfADCConversionCycles = 0; // Performed by special debugging hook in firmware
long nCycles = 0; long nElapsedTime;
long nFilteredRawValueSum = 0;
long nGyroValue; volatile long nFilteredRawValue; long nNonJitterCycles = 0;
typedef enum { biasMinus6orLess, biasMinus5, biasMinus4, biasMinus3, biasMinus2, biasMinus1, bias, biasPlus1, biasPlus2, biasPlus3, biasPlus4, biasPlus5, biasPlus6orMore, } TCountsIndices;
long nGyroRawAnalogValue; long nGyroBias; long nSensorSmallBias; // Performed by special debugging hook in firmware long nGyroSensorScale; long nGyroFullCount; long nCounts[(TCountsIndices) 13]; short nMaxValueCurr10Seconds; short nMinValueCurr10Seconds; short nPrev10SecondTime = -1; short nCurr10SecondTime; const int kHistogramSize = 600; short nMaxValues[kHistogramSize]; short nMinValues[kHistogramSize]; long nLoopStartTime; long nPrevTime; long nCurrTime;
const int kJitterIgnoreRange = 4;
task main() { // Firmware retains sensor type setting between invocations of program. Explicitly setting to 'sensorNone' and // then back to 'sensorGyro' will explicitly force the firmware driver to recalculate the biase settings. Which // you may want to do via the Debugger's "STOP" and "START" buttons.
SensorType[gyro] = sensorNone; SensorType[gyro] = sensorGyro;
memset(&nCounts[0], 0, sizeof(nCounts)); memset(&nMaxValues, 0, sizeof(nMaxValues)); memset(&nMinValues, 0, sizeof(nMinValues)); nNumbOfADCConversionCycles = 0; nFilteredRawValueSum = 0; while (SensorBias[gyro] == 0) {} SensorValue[gyro] = 0;
nGyroBias = SensorBias[gyro]; nSensorSmallBias = SensorSmallBias[gyro]; nGyroSensorScale = SensorScale[gyro]; nGyroFullCount = SensorFullCount[gyro];
nLoopStartTime = nPgmTime; while (true) { short nDifference; ++nCycles; nGyroValue = SensorValue[gyro]; nGyroRawAnalogValue = getSensorRawADValue(gyro);
nDifference = nGyroRawAnalogValue - nGyroBias; if ((nDifference < -kJitterIgnoreRange) || (nDifference > +kJitterIgnoreRange)) { nFilteredRawValueSum += nDifference; ++nNonJitterCycles; if ((nNonJitterCycles % 1024) == 0) nFilteredRawValueSum -= nSensorSmallBias; } if (nDifference < -6) nDifference = -6; else if (nDifference > +6) nDifference = +6; ++nCounts[bias + nDifference];
nFilteredRawValue = nFilteredRawValueSum / nGyroSensorScale;
// Calculate statistics and value histogram
nCurrTime = nPgmTime; nElapsedTime = nCurrTime - nLoopStartTime; nCurr10SecondTime = nElapsedTime / 10000; if (nCurr10SecondTime != nPrev10SecondTime) { if (nPrev10SecondTime >= 0) { nMaxValues[nPrev10SecondTime] = nMaxValueCurr10Seconds; nMinValues[nPrev10SecondTime] = nMinValueCurr10Seconds; } nMaxValueCurr10Seconds = nGyroValue; nMinValueCurr10Seconds = nGyroValue; } if (nGyroValue > nMaxValueCurr10Seconds) nMaxValueCurr10Seconds = nGyroValue; else if (nGyroValue < nMinValueCurr10Seconds) nMinValueCurr10Seconds = nGyroValue; nPrev10SecondTime = nCurr10SecondTime;
while (nCurrTime == nPrevTime) { nCurrTime = nPgmTime; } nPrevTime = nCurrTime; //wait1Msec(1); } }
|