/* Code governing the gyroscope Jan. 9, 2005 */ int gyro_type = 300; /* 150 for ADXRS150, 300 for ADXRS300 */ float lsb_ms_per_deg = 256.0; /* nominal scale factor depending on gyro type */ float scale_factor = 1.017; /* adjust this for your particular gyro! */ float theta; /* current angle, units explained in update_angle() */ float offset; /* gyro offset */ int num_avgs = 800; /* number of averages for calibration */ long time_ms; /* system time the last gyro reading was taken */ 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 = 3000L; 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); samples++; } offset = sum / (float)samples; lsb_ms_per_deg *= scale_factor; theta = 0.0; } void update_angle(){ /* The units of theta are converter LSBs * ms 1 deg/s = 5mV = 0.256 LSB for ADXRS300, 12.5mV = 0.64LSB for ADXRS150 1 deg = 0.256LSB * 1000ms = 256 (stored in lsb_ms_per_deg) To convert theta to degrees, divide by lsb_ms_per_deg. */ long delta_t_ms, new_time_ms; int i; time_ms = mseconds(); for(;;){ new_time_ms = mseconds(); delta_t_ms = (new_time_ms - time_ms); /* This does the Riemann sum; CCW gyro output polarity is negative when the gyro is visible from the top of the robot. */ theta -= ((float)analog(GYRO) - offset) * (float)delta_t_ms; time_ms = new_time_ms; defer(); } } void display_angle(){ for(;;){ printf("%d degrees\n",(int)(theta / lsb_ms_per_deg)); msleep(200L); defer(); } }