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