/* Gyro demo program for gyro with noise injection.
   Howard Samuels, 11/16/2004 
   howard.samuels@analog.com
*/
#define GLOBAL 1
#define LOCAL  2
 
float lsb_ms_per_deg = 256.0*1.017;       /* nominal scale factor depending on gyro type */
float theta;
float offset;               /* gyro offset */
int gyro_port = 2;          /* gyro analog input port number */
long time_ms;               /* system time the last gyro reading was taken */

int left = 0;               /* motor ports */
int right = 2;

void calibrate_gyro1()
{
    offset = 132.4;
    theta = 0.0;
}

void calibrate_gyro()
{
/*
  Averaging a number of readings only works when the gyro 
  noise is greater than the converter resolution.  Since 
  the xrs300 noise is far lower than the 20mV resolution 
  of the 8-bit converter, a noise source has been added 
  to the gyro board.  You should average for about 5 
  seconds to minimize the effects of the added noise.
*/

    int samples;
    float sum;
    long start_time_ms;
    long int_time_ms = 5000L;

        /*printf("Place robot,    press start.\n");
          start_press();*/
    printf("Stabilizing...\n");
    msleep(500L); /* Wait for robot to stabilize mechanically */
    printf("Calibrating     offset...\n");

        /* average some samples for offset */
    sum = 0.0;
    samples = 0;
    start_time_ms = mseconds();
    while( ( mseconds() - start_time_ms ) < int_time_ms ){
        sum += (float)analog(gyro_port);
        samples++;
    }
    offset = sum / (float)samples;

    theta = 0.0;
} /* calibrate_gyro() */

void spin(int speed){
    motor(left, -speed);
    motor(right, speed);
} /* spin() */