static void displayStatusAndTime()
{
displayLCDPos(1, 0);
if (bIfiRobotDisabled)
displayNextLCDString("Disable ");
else
{
if (bIfiAutonomousMode)
displayNextLCDString("Auton ");
else
displayNextLCDString("Driver ");
}
displayNextLCDNumber(nTimeXX / 600, 2);
displayNextLCDChar(':');
displayNextLCDNumber((nTimeXX / 10) % 60, -2);
displayNextLCDChar('.');
displayNextLCDNumber(nTimeXX % 10, 1);
}
task main()
{
int i;
const int kNumbOfMainSamples = 4000;
bLCDBacklight = true;
displayLCDPos(0, 0);
displayNextLCDString("Gyro Noise Test ");
memset(nSampleHisogram[0], 0, sizeof(nSampleHisogram));
wait1Msec(1000);
// Calculate a rough mezsure of 'bias' sum that fits in 16-bit signed.
nSampleSum = 0;
const int kRoughBiasCounts = 30;
for (i = 0; i < kRoughBiasCounts; ++i)
{
nSampleSum += SensorValue[gyro];
wait1Msec(1);
}
nBiasRough = nSampleSum / kRoughBiasCounts;
nSampleSum = 0;
for (i = 0; i < kNumbOfMainSamples; ++i)
{
nOffset = SensorValue[gyro] - nBiasRough;
nSampleSum += nOffset;
wait1Msec(1);
}
nBias = nBiasRough + nSampleSum / kNumbOfMainSamples;
nBiasSmall = nSampleSum / (kNumbOfMainSamples / 100) - (nSampleSum / kNumbOfMainSamples) * 100;
nBiasSmaller = nSampleSum / (kNumbOfMainSamples / 1000) - nBiasSmall * 10;
StartTask(testMotorNoise);
nLastCycles = -1;
bool bDisplayIt;
bool bOverflow = false;
for (nCycles = 0; true; ++nCycles)
{
//ASSERT(nCycles == (nLastCycles + 1));
nLastCycles = nCycles;
nTimeStamp = nPgmTime;
nOffset = SensorValue[gyro] - nBias;
nDrift += nOffset;
if (nCycles >= 0)
{
if ((nCycles % 100) == 50)
nDrift -= nBiasSmall;
if ((nCycles % 1000) == 500)
{
nDrift -= nBiasSmaller;
bDisplayIt = true;
}
else
bDisplayIt = false;
}
else
{
if ((nCycles % 1000) == -50)
nDrift -= nBiasSmall;
if ((nCycles % 1000) == -500)
{
nDrift -= nBiasSmaller;
bDisplayIt = true;
}
else
bDisplayIt = false;
}
if (nOffset < -32)
nOffset = -32;
else if (nOffset > 32)
nOffset = 32;
nCount = nSampleHisogram[nOffset + 32];
if (nCount >= 32767)
bOverflow = true;
if (!bOverflow)
++nSampleHisogram[nOffset + 32];
if (true)
{
nPlusMinusWeighted += nOffset;
if (nOffset < 0)
--nPlusMinus;
else if (nOffset > 0)
++nPlusMinus;
}
if (bDisplayIt)
{
++nSeconds;
displayLCDPos(1, 0);
displayNextLCDNumber(nSeconds, 3);
displayNextLCDString(" : ");
if (nDrift > 0)
{
displayNextLCDNumber(nDrift / 1000);
displayNextLCDChar('.');
displayNextLCDNumber(nDrift % 1000, -3);
}
else
{
displayNextLCDChar('-');
displayNextLCDNumber(- nDrift / 1000);
displayNextLCDChar('.');
displayNextLCDNumber(- nDrift % 1000, -3);
}
displayNextLCDChar(' ');
displayNextLCDChar(' ');
}
while (nTimeStamp == nPgmTime)
{}
}
StopTask(testMotorNoise);
}
