main.cpp

00001 /**
00002  * IMU filter example.
00003  *
00004  * Calculate the roll, pitch and yaw angles.
00005  */
00006 #include "IMUfilter.h"
00007 #include "ADXL345_I2C.h"
00008 #include "ITG3200.h"
00009 #include "USBMouse.h"
00010 
00011 
00012 #define MOVERATE 1
00013 #define MAXMOVE 100
00014 #define MOVETHRESHOLD 3
00015 //Gravity at Earth's surface in m/s/s
00016 #define g0 9.812865328
00017 //Number of samples to average.
00018 #define SAMPLES 2
00019 //Number of samples to be averaged for a null bias calculation
00020 //during calibration.
00021 #define CALIBRATION_SAMPLES 32
00022 //Convert from radians to degrees.
00023 #define toDegrees(x) (x * 57.2957795)
00024 //Convert from degrees to radians.
00025 #define toRadians(x) (x * 0.01745329252)
00026 //ITG-3200 sensitivity is 14.375 LSB/(degrees/sec).
00027 #define GYROSCOPE_GAIN (1 / 14.375)
00028 //Full scale resolution on the ADXL345 is 4mg/LSB.
00029 #define ACCELEROMETER_GAIN (0.000061035 * g0)
00030 //Sampling gyroscope at .
00031 #define GYRO_RATE   0.005
00032 //Sampling accelerometer at 
00033 #define ACC_RATE    0.005
00034 #define FILTER_RATE 0.015
00035 
00036 DigitalIn leftClick(p16);
00037 DigitalIn rightClick(p15);
00038 
00039 Serial pc(USBTX, USBRX);
00040 //At rest the gyroscope is centred around 0 and goes between about
00041 //-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly
00042 //5/15 = 0.3 degrees/sec.
00043 IMUfilter imuFilter(FILTER_RATE, 0.3);
00044 ADXL345_I2C accelerometer(p28, p27);
00045 ITG3200 gyroscope(p28, p27);
00046 USBMouse mouse;
00047 
00048 Ticker accelerometerTicker;
00049 Ticker gyroscopeTicker;
00050 Ticker filterTicker;
00051 
00052 /**
00053  * IMU filter example.
00054  *
00055  * Calculate the roll, pitch and yaw angles.
00056  */
00057  
00058 
00059 
00060 //Offsets for the gyroscope.
00061 //The readings we take when the gyroscope is stationary won't be 0, so we'll
00062 //average a set of readings we do get when the gyroscope is stationary and
00063 //take those away from subsequent readings to ensure the gyroscope is offset
00064 //or "biased" to 0.
00065 double w_xBias;
00066 double w_yBias;
00067 double w_zBias;
00068 
00069 //Offsets for the accelerometer.
00070 //Same as with the gyroscope.
00071 double a_xBias;
00072 double a_yBias;
00073 double a_zBias;
00074 
00075 //Accumulators used for oversampling and then averaging.
00076 volatile double a_xAccumulator = 0;
00077 volatile double a_yAccumulator = 0;
00078 volatile double a_zAccumulator = 0;
00079 volatile double w_xAccumulator = 0;
00080 volatile double w_yAccumulator = 0;
00081 volatile double w_zAccumulator = 0;
00082 
00083 //Accelerometer and gyroscope readings for x, y, z axes.
00084 volatile double a_x;
00085 volatile double a_y;
00086 volatile double a_z;
00087 volatile double w_x;
00088 volatile double w_y;
00089 volatile double w_z;
00090 
00091 //Buffer for accelerometer readings.
00092 int readings[3];
00093 //Number of accelerometer samples we're on.
00094 int accelerometerSamples = 0;
00095 //Number of gyroscope samples we're on.
00096 int gyroscopeSamples = 0;
00097 
00098 /**
00099  * Prototypes
00100  */
00101 //Set up the ADXL345 appropriately.
00102 void initializeAcceleromter(void);
00103 //Calculate the null bias.
00104 void calibrateAccelerometer(void);
00105 //Take a set of samples and average them.
00106 void sampleAccelerometer(void);
00107 //Set up the ITG3200 appropriately.
00108 void initializeGyroscope(void);
00109 //Calculate the null bias.
00110 void calibrateGyroscope(void);
00111 //Take a set of samples and average them.
00112 void sampleGyroscope(void);
00113 //Update the filter and calculate the Euler angles.
00114 void filter(void);
00115 void processMove(void);
00116 void processClick(void);
00117 
00118 void initializeAccelerometer(void) {
00119 
00120     //Go into standby mode to configure the device.
00121     accelerometer.setPowerControl(0x00);
00122     //Full resolution, +/-16g, 4mg/LSB.
00123     accelerometer.setDataFormatControl(0x0B);
00124     //200Hz data rate.
00125     accelerometer.setDataRate(ADXL345_1600HZ);
00126     //Measurement mode.
00127     accelerometer.setPowerControl(0x08);
00128     //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
00129     wait_ms(22);
00130 
00131 }
00132 
00133 void initializeGyroscope(void) {
00134 
00135     //Low pass filter bandwidth of 42Hz.
00136     gyroscope.setLpBandwidth(LPFBW_42HZ);
00137     gyroscope.setSampleRateDivider(0);
00138     pc.printf("%d\n", gyroscope.getSampleRateDivider());
00139     pc.printf("%d\n", gyroscope.getInternalSampleRate());
00140     wait_ms(22);
00141 }
00142 
00143 
00144 void calibrateAccelerometer(void) {
00145     
00146     pc.printf("Calibrating Accelerometer\n");
00147     a_xAccumulator = 0;
00148     a_yAccumulator = 0;
00149     a_zAccumulator = 0;
00150     
00151     //Take a number of readings and average them
00152     //to calculate the zero g offset.
00153     for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
00154     
00155     accelerometer.getOutput(readings);
00156     a_xAccumulator += (int16_t) readings[0];
00157     a_yAccumulator += (int16_t) readings[1];
00158     a_zAccumulator += (int16_t) readings[2];
00159 
00160 
00161     wait(ACC_RATE);
00162 
00163     }
00164 
00165     a_xAccumulator /= CALIBRATION_SAMPLES;
00166     a_yAccumulator /= CALIBRATION_SAMPLES;
00167     a_zAccumulator /= CALIBRATION_SAMPLES;
00168 
00169     //At 4mg/LSB, 250 LSBs is 1g.
00170     a_xBias = a_xAccumulator;
00171     a_yBias = a_yAccumulator;
00172     a_zBias = (a_zAccumulator - 981);
00173 
00174     a_xAccumulator = 0;
00175     a_yAccumulator = 0;
00176     a_zAccumulator = 0;
00177     pc.printf("Calibration Complete\n");
00178 }
00179 
00180 
00181 
00182 void calibrateGyroscope(void) {
00183     
00184     pc.printf("Calibrating Gyro\n");
00185     w_xAccumulator = 0;
00186     w_yAccumulator = 0;
00187     w_zAccumulator = 0;
00188 
00189     //Take a number of readings and average them
00190     //to calculate the gyroscope bias offset.
00191     for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
00192 
00193         w_xAccumulator += gyroscope.getGyroX();
00194         w_yAccumulator += gyroscope.getGyroY();
00195         w_zAccumulator += gyroscope.getGyroZ();
00196         wait(GYRO_RATE);
00197     }
00198 
00199     //Average the samples.
00200     w_xAccumulator /= CALIBRATION_SAMPLES;
00201     w_yAccumulator /= CALIBRATION_SAMPLES;
00202     w_zAccumulator /= CALIBRATION_SAMPLES;
00203 
00204     w_xBias = w_xAccumulator;
00205     w_yBias = w_yAccumulator;
00206     w_zBias = w_zAccumulator;
00207 
00208     w_xAccumulator = 0;
00209     w_yAccumulator = 0;
00210     w_zAccumulator = 0;
00211     
00212     pc.printf("Calibration Complete\n");
00213 }
00214 
00215 
00216 void sampleAccelerometer(void) {
00217 
00218     //Have we taken enough samples?
00219     if (accelerometerSamples == SAMPLES) {
00220 
00221         //Average the samples, remove the bias, and calculate the acceleration
00222         //in m/s/s.
00223         a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
00224         a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
00225         a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
00226 
00227         a_xAccumulator = 0;
00228         a_yAccumulator = 0;
00229         a_zAccumulator = 0;
00230         accelerometerSamples = 0;
00231 
00232     } else {
00233         //Take another sample.
00234         accelerometer.getOutput(readings);
00235         a_xAccumulator += (int16_t) readings[0];
00236         a_yAccumulator += (int16_t) readings[1];
00237         a_zAccumulator += (int16_t) readings[2];
00238 
00239         accelerometerSamples++;
00240         //pc.printf("Sample Accl %d", accelerometerSamples);
00241     }
00242 
00243 }
00244 
00245 
00246 void sampleGyroscope(void) {
00247 
00248     //Have we taken enough samples?
00249     if (gyroscopeSamples == SAMPLES) {
00250 
00251         //Average the samples, remove the bias, and calculate the angular
00252         //velocity in rad/s.
00253         w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
00254         w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
00255         w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
00256 
00257         w_xAccumulator = 0;
00258         w_yAccumulator = 0;
00259         w_zAccumulator = 0;
00260         gyroscopeSamples = 0;
00261 
00262     } else {
00263         //Take another sample.
00264         w_xAccumulator += gyroscope.getGyroX();
00265         w_yAccumulator += gyroscope.getGyroY();
00266         w_zAccumulator += gyroscope.getGyroZ();
00267 
00268         gyroscopeSamples++;
00269         //pc.printf("Sample Gyro %d", gyroscopeSamples);
00270     }
00271 
00272 }
00273 
00274 void filter(void) {
00275     //Update the filter variables.
00276     imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
00277     //Calculate the new Euler angles.
00278     imuFilter.computeEuler();
00279 
00280 }
00281 
00282 void processClick()
00283 {
00284      static bool preRightClick = false;  
00285 
00286      if (leftClick == 0)   
00287      {  
00288         mouse.press(MOUSE_LEFT);
00289      }  
00290      else
00291      {  
00292         mouse.release(MOUSE_LEFT);          
00293      }
00294 
00295      // Right Mouse Click  ___  Falling Edge Detection
00296      if (rightClick == 0 && preRightClick == false)
00297      {  
00298          preRightClick = true;
00299      } 
00300      else if (rightClick == 1 && preRightClick == true)
00301      {   preRightClick = false;
00302          mouse.click(MOUSE_RIGHT);      
00303      }  
00304 }
00305 
00306 
00307 void processMove(void)
00308 {    
00309     int16_t move_x, move_y;
00310         
00311         
00312     move_x = (int16_t)(-MOVERATE*toDegrees(imuFilter.getRoll()));
00313     move_y = (int16_t)(-MOVERATE*toDegrees(imuFilter.getPitch()));      
00314 
00315     if (move_x <= MOVETHRESHOLD && move_x >= -MOVETHRESHOLD)
00316          move_x = 0;
00317     else if (move_x > MOVETHRESHOLD){
00318          if (move_x > MAXMOVE+MOVETHRESHOLD) move_x = MAXMOVE;
00319           else move_x -=MOVETHRESHOLD;
00320      }
00321             
00322     else{
00323          if (move_x < -MAXMOVE-MOVETHRESHOLD) move_x = -MAXMOVE;    
00324            else move_x+=MOVETHRESHOLD;
00325     } 
00326             
00327         
00328         if (move_y <= MOVETHRESHOLD && move_y  >= -MOVETHRESHOLD)
00329             move_y = 0;
00330         else if (move_y > MOVETHRESHOLD){
00331             if (move_y > MAXMOVE+MOVETHRESHOLD) move_y = MAXMOVE;
00332             else move_y -=MOVETHRESHOLD;
00333         }
00334             
00335         else{
00336             if (move_y < -MAXMOVE-MOVETHRESHOLD) move_y = -MAXMOVE;    
00337             else move_y+=MOVETHRESHOLD;
00338         } 
00339                    
00340         pc.printf("move_x = %d,  move_ y = %d\n", move_x,move_y);  
00341                 
00342         mouse.move(move_x, move_y);            
00343    }
00344 
00345 int main() {
00346 
00347     //pc.printf("Starting IMU filter test...\n");
00348 
00349     //Initialize inertial sensors.
00350     initializeAccelerometer();
00351     calibrateAccelerometer();
00352     initializeGyroscope();
00353     calibrateGyroscope();
00354 
00355    
00356     
00357     leftClick.mode(PullUp); 
00358     rightClick.mode(PullUp); 
00359     //pc.printf("Initialized Successfully...\n\r");
00360 
00361     //Set up timers.
00362     //Accelerometer data rate is 200Hz, so we'll sample at this speed.
00363     accelerometerTicker.attach(&sampleAccelerometer, GYRO_RATE);
00364     //Gyroscope data rate is 200Hz, so we'll sample at this speed.
00365     gyroscopeTicker.attach(&sampleGyroscope, ACC_RATE);
00366     //Update the filter variables at the correct rate.
00367     filterTicker.attach(&filter, FILTER_RATE);
00368    
00369    
00370    //pc.printf("Timers Setup...Entering Loop...\n\r");
00371      
00372    
00373     while (1) {
00374 
00375         wait(0.01);
00376         pc.printf("angle_x = %f,  angle_ y = %f\n",-toDegrees(imuFilter.getRoll()),-toDegrees(imuFilter.getPitch()));   
00377         processClick();
00378         processMove();
00379     }
00380 
00381 }