Justin Eng / Mbed 2 deprecated RoboticsCatAndMouse_AutonomousDriver

Spring 2014, ECE 4180 project, Georgia Institute of Technolgoy. This is the autonomous driver program for the Robotics Cat and Mouse program.

Dependencies:   IMUfilter ADXL345_I2C mbed ITG3200 USBHost mbed-rtos

IMU_RPY.h@3:0a6e4d139b86, 2014-04-30 (annotated)

Committer:
Strikewolf
Date:
Wed Apr 30 12:25:47 2014 +0000
Revision:
3:0a6e4d139b86
Parent:
1:dacf7db790f6 
Final product before demo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Strikewolf 1:dacf7db790f6 1 /**
Strikewolf 1:dacf7db790f6 2 * IMU filter example.
Strikewolf 1:dacf7db790f6 3 *
Strikewolf 1:dacf7db790f6 4 * Calculate the roll, pitch and yaw angles.
Strikewolf 1:dacf7db790f6 5 */
Strikewolf 1:dacf7db790f6 6 #include "IMUfilter.h"
Strikewolf 1:dacf7db790f6 7 #include "ADXL345_I2C.h"
Strikewolf 1:dacf7db790f6 8 #include "ITG3200.h"
Strikewolf 1:dacf7db790f6 9
Strikewolf 1:dacf7db790f6 10 //Gravity at Earth's surface in m/s/s
Strikewolf 1:dacf7db790f6 11 #define g0 9.812865328
Strikewolf 1:dacf7db790f6 12 //Number of samples to average.
Strikewolf 1:dacf7db790f6 13 #define SAMPLES 4
Strikewolf 1:dacf7db790f6 14 //Number of samples to be averaged for a null bias calculation
Strikewolf 1:dacf7db790f6 15 //during calibration.
Strikewolf 1:dacf7db790f6 16 #define CALIBRATION_SAMPLES 32
Strikewolf 1:dacf7db790f6 17 //Convert from radians to degrees.
Strikewolf 1:dacf7db790f6 18 #define toDegrees(x) (x * 57.2957795)
Strikewolf 1:dacf7db790f6 19 //Convert from degrees to radians.
Strikewolf 1:dacf7db790f6 20 #define toRadians(x) (x * 0.01745329252)
Strikewolf 1:dacf7db790f6 21 //ITG-3200 sensitivity is 14.375 LSB/(degrees/sec).
Strikewolf 1:dacf7db790f6 22 #define GYROSCOPE_GAIN (1 / 14.375)
Strikewolf 1:dacf7db790f6 23 //Full scale resolution on the ADXL345 is 4mg/LSB.
Strikewolf 1:dacf7db790f6 24 #define ACCELEROMETER_GAIN (0.004 * g0)
Strikewolf 1:dacf7db790f6 25 //Sampling gyroscope at 200Hz.
Strikewolf 1:dacf7db790f6 26 #define GYRO_RATE 0.005
Strikewolf 1:dacf7db790f6 27 //Sampling accelerometer at 200Hz.
Strikewolf 1:dacf7db790f6 28 #define ACC_RATE 0.005
Strikewolf 1:dacf7db790f6 29 //Updating filter at 40Hz.
Strikewolf 1:dacf7db790f6 30 #define FILTER_RATE 0.1
Strikewolf 1:dacf7db790f6 31
Strikewolf 1:dacf7db790f6 32 //At rest the gyroscope is centred around 0 and goes between about
Strikewolf 1:dacf7db790f6 33 //-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly
Strikewolf 1:dacf7db790f6 34 //5/15 = 0.3 degrees/sec.
Strikewolf 1:dacf7db790f6 35 IMUfilter imuFilter(FILTER_RATE, 0.3);
Strikewolf 1:dacf7db790f6 36 ADXL345_I2C accelerometer(p9, p10);
Strikewolf 1:dacf7db790f6 37 ITG3200 gyroscope(p9, p10);
Strikewolf 1:dacf7db790f6 38 Ticker accelerometerTicker;
Strikewolf 1:dacf7db790f6 39 Ticker gyroscopeTicker;
Strikewolf 1:dacf7db790f6 40 Ticker filterTicker;
Strikewolf 1:dacf7db790f6 41
Strikewolf 1:dacf7db790f6 42 //Offsets for the gyroscope.
Strikewolf 1:dacf7db790f6 43 //The readings we take when the gyroscope is stationary won't be 0, so we'll
Strikewolf 1:dacf7db790f6 44 //average a set of readings we do get when the gyroscope is stationary and
Strikewolf 1:dacf7db790f6 45 //take those away from subsequent readings to ensure the gyroscope is offset
Strikewolf 1:dacf7db790f6 46 //or "biased" to 0.
Strikewolf 1:dacf7db790f6 47 double w_xBias;
Strikewolf 1:dacf7db790f6 48 double w_yBias;
Strikewolf 1:dacf7db790f6 49 double w_zBias;
Strikewolf 1:dacf7db790f6 50
Strikewolf 1:dacf7db790f6 51 //Offsets for the accelerometer.
Strikewolf 1:dacf7db790f6 52 //Same as with the gyroscope.
Strikewolf 1:dacf7db790f6 53 double a_xBias;
Strikewolf 1:dacf7db790f6 54 double a_yBias;
Strikewolf 1:dacf7db790f6 55 double a_zBias;
Strikewolf 0:84d5aa80fd77 56
Strikewolf 1:dacf7db790f6 57 //Accumulators used for oversampling and then averaging.
Strikewolf 1:dacf7db790f6 58 volatile double a_xAccumulator = 0;
Strikewolf 1:dacf7db790f6 59 volatile double a_yAccumulator = 0;
Strikewolf 1:dacf7db790f6 60 volatile double a_zAccumulator = 0;
Strikewolf 1:dacf7db790f6 61 volatile double w_xAccumulator = 0;
Strikewolf 1:dacf7db790f6 62 volatile double w_yAccumulator = 0;
Strikewolf 1:dacf7db790f6 63 volatile double w_zAccumulator = 0;
Strikewolf 1:dacf7db790f6 64
Strikewolf 1:dacf7db790f6 65 //Accelerometer and gyroscope readings for x, y, z axes.
Strikewolf 1:dacf7db790f6 66 volatile double a_x;
Strikewolf 1:dacf7db790f6 67 volatile double a_y;
Strikewolf 1:dacf7db790f6 68 volatile double a_z;
Strikewolf 1:dacf7db790f6 69 volatile double w_x;
Strikewolf 1:dacf7db790f6 70 volatile double w_y;
Strikewolf 1:dacf7db790f6 71 volatile double w_z;
Strikewolf 1:dacf7db790f6 72
Strikewolf 1:dacf7db790f6 73 //Buffer for accelerometer readings.
Strikewolf 1:dacf7db790f6 74 int readings[3];
Strikewolf 1:dacf7db790f6 75 //Number of accelerometer samples we're on.
Strikewolf 1:dacf7db790f6 76 int accelerometerSamples = 0;
Strikewolf 1:dacf7db790f6 77 //Number of gyroscope samples we're on.
Strikewolf 1:dacf7db790f6 78 int gyroscopeSamples = 0;
Strikewolf 1:dacf7db790f6 79
Strikewolf 1:dacf7db790f6 80 /**
Strikewolf 1:dacf7db790f6 81 * Prototypes
Strikewolf 1:dacf7db790f6 82 */
Strikewolf 1:dacf7db790f6 83 //Set up the ADXL345 appropriately.
Strikewolf 1:dacf7db790f6 84 void initializeAcceleromter(void);
Strikewolf 1:dacf7db790f6 85 //Calculate the null bias.
Strikewolf 1:dacf7db790f6 86 void calibrateAccelerometer(void);
Strikewolf 1:dacf7db790f6 87 //Take a set of samples and average them.
Strikewolf 1:dacf7db790f6 88 void sampleAccelerometer(void);
Strikewolf 1:dacf7db790f6 89 //Set up the ITG3200 appropriately.
Strikewolf 1:dacf7db790f6 90 void initializeGyroscope(void);
Strikewolf 1:dacf7db790f6 91 //Calculate the null bias.
Strikewolf 1:dacf7db790f6 92 void calibrateGyroscope(void);
Strikewolf 1:dacf7db790f6 93 //Take a set of samples and average them.
Strikewolf 1:dacf7db790f6 94 void sampleGyroscope(void);
Strikewolf 1:dacf7db790f6 95 //Update the filter and calculate the Euler angles.
Strikewolf 1:dacf7db790f6 96 void filter(void);
Strikewolf 1:dacf7db790f6 97
Strikewolf 1:dacf7db790f6 98 void initializeAccelerometer(void)
Strikewolf 1:dacf7db790f6 99 {
Strikewolf 1:dacf7db790f6 100
Strikewolf 1:dacf7db790f6 101 //Go into standby mode to configure the device.
Strikewolf 1:dacf7db790f6 102 accelerometer.setPowerControl(0x00);
Strikewolf 1:dacf7db790f6 103 //Full resolution, +/-16g, 4mg/LSB.
Strikewolf 1:dacf7db790f6 104 accelerometer.setDataFormatControl(0x0B);
Strikewolf 1:dacf7db790f6 105 //200Hz data rate.
Strikewolf 1:dacf7db790f6 106 accelerometer.setDataRate(ADXL345_200HZ);
Strikewolf 1:dacf7db790f6 107 //Measurement mode.
Strikewolf 1:dacf7db790f6 108 accelerometer.setPowerControl(0x08);
Strikewolf 1:dacf7db790f6 109 //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
Strikewolf 1:dacf7db790f6 110 wait_ms(22);
Strikewolf 1:dacf7db790f6 111
Strikewolf 1:dacf7db790f6 112 }
Strikewolf 1:dacf7db790f6 113
Strikewolf 1:dacf7db790f6 114 void sampleAccelerometer(void)
Strikewolf 1:dacf7db790f6 115 {
Strikewolf 1:dacf7db790f6 116
Strikewolf 1:dacf7db790f6 117 //Have we taken enough samples?
Strikewolf 1:dacf7db790f6 118 if (accelerometerSamples == SAMPLES) {
Strikewolf 1:dacf7db790f6 119
Strikewolf 1:dacf7db790f6 120 //Average the samples, remove the bias, and calculate the acceleration
Strikewolf 1:dacf7db790f6 121 //in m/s/s.
Strikewolf 1:dacf7db790f6 122 a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
Strikewolf 1:dacf7db790f6 123 a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
Strikewolf 1:dacf7db790f6 124 a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
Strikewolf 1:dacf7db790f6 125
Strikewolf 0:84d5aa80fd77 126 a_xAccumulator = 0;
Strikewolf 0:84d5aa80fd77 127 a_yAccumulator = 0;
Strikewolf 0:84d5aa80fd77 128 a_zAccumulator = 0;
Strikewolf 1:dacf7db790f6 129 accelerometerSamples = 0;
Strikewolf 1:dacf7db790f6 130
Strikewolf 1:dacf7db790f6 131 } else {
Strikewolf 1:dacf7db790f6 132 //Take another sample.
Strikewolf 1:dacf7db790f6 133 accelerometer.getOutput(readings);
Strikewolf 1:dacf7db790f6 134
Strikewolf 1:dacf7db790f6 135 a_xAccumulator += (int16_t) readings[0];
Strikewolf 1:dacf7db790f6 136 a_yAccumulator += (int16_t) readings[1];
Strikewolf 1:dacf7db790f6 137 a_zAccumulator += (int16_t) readings[2];
Strikewolf 1:dacf7db790f6 138
Strikewolf 1:dacf7db790f6 139 accelerometerSamples++;
Strikewolf 1:dacf7db790f6 140
Strikewolf 1:dacf7db790f6 141 }
Strikewolf 1:dacf7db790f6 142
Strikewolf 1:dacf7db790f6 143 }
Strikewolf 1:dacf7db790f6 144
Strikewolf 1:dacf7db790f6 145 void calibrateAccelerometer(void)
Strikewolf 1:dacf7db790f6 146 {
Strikewolf 1:dacf7db790f6 147
Strikewolf 1:dacf7db790f6 148 a_xAccumulator = 0;
Strikewolf 1:dacf7db790f6 149 a_yAccumulator = 0;
Strikewolf 1:dacf7db790f6 150 a_zAccumulator = 0;
Strikewolf 1:dacf7db790f6 151
Strikewolf 1:dacf7db790f6 152 //Take a number of readings and average them
Strikewolf 1:dacf7db790f6 153 //to calculate the zero g offset.
Strikewolf 1:dacf7db790f6 154 for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
Strikewolf 1:dacf7db790f6 155
Strikewolf 1:dacf7db790f6 156 accelerometer.getOutput(readings);
Strikewolf 1:dacf7db790f6 157
Strikewolf 1:dacf7db790f6 158 a_xAccumulator += (int16_t) readings[0];
Strikewolf 1:dacf7db790f6 159 a_yAccumulator += (int16_t) readings[1];
Strikewolf 1:dacf7db790f6 160 a_zAccumulator += (int16_t) readings[2];
Strikewolf 1:dacf7db790f6 161
Strikewolf 1:dacf7db790f6 162 wait(ACC_RATE);
Strikewolf 1:dacf7db790f6 163
Strikewolf 0:84d5aa80fd77 164 }
Strikewolf 1:dacf7db790f6 165
Strikewolf 1:dacf7db790f6 166 a_xAccumulator /= CALIBRATION_SAMPLES;
Strikewolf 1:dacf7db790f6 167 a_yAccumulator /= CALIBRATION_SAMPLES;
Strikewolf 1:dacf7db790f6 168 a_zAccumulator /= CALIBRATION_SAMPLES;
Strikewolf 1:dacf7db790f6 169
Strikewolf 1:dacf7db790f6 170 //At 4mg/LSB, 250 LSBs is 1g.
Strikewolf 1:dacf7db790f6 171 a_xBias = a_xAccumulator;
Strikewolf 1:dacf7db790f6 172 a_yBias = a_yAccumulator;
Strikewolf 1:dacf7db790f6 173 a_zBias = (a_zAccumulator - 250);
Strikewolf 1:dacf7db790f6 174
Strikewolf 1:dacf7db790f6 175 a_xAccumulator = 0;
Strikewolf 1:dacf7db790f6 176 a_yAccumulator = 0;
Strikewolf 1:dacf7db790f6 177 a_zAccumulator = 0;
Strikewolf 1:dacf7db790f6 178
Strikewolf 1:dacf7db790f6 179 }
Strikewolf 1:dacf7db790f6 180
Strikewolf 1:dacf7db790f6 181 void initializeGyroscope(void)
Strikewolf 1:dacf7db790f6 182 {
Strikewolf 1:dacf7db790f6 183
Strikewolf 1:dacf7db790f6 184 //Low pass filter bandwidth of 42Hz.
Strikewolf 1:dacf7db790f6 185 gyroscope.setLpBandwidth(LPFBW_42HZ);
Strikewolf 1:dacf7db790f6 186 //Internal sample rate of 200Hz. (1kHz / 5).
Strikewolf 1:dacf7db790f6 187 gyroscope.setSampleRateDivider(4);
Strikewolf 1:dacf7db790f6 188
Strikewolf 1:dacf7db790f6 189 }
Strikewolf 1:dacf7db790f6 190
Strikewolf 1:dacf7db790f6 191 void calibrateGyroscope(void)
Strikewolf 1:dacf7db790f6 192 {
Strikewolf 1:dacf7db790f6 193
Strikewolf 1:dacf7db790f6 194 w_xAccumulator = 0;
Strikewolf 1:dacf7db790f6 195 w_yAccumulator = 0;
Strikewolf 1:dacf7db790f6 196 w_zAccumulator = 0;
Strikewolf 1:dacf7db790f6 197
Strikewolf 1:dacf7db790f6 198 //Take a number of readings and average them
Strikewolf 1:dacf7db790f6 199 //to calculate the gyroscope bias offset.
Strikewolf 1:dacf7db790f6 200 for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
Strikewolf 1:dacf7db790f6 201
Strikewolf 1:dacf7db790f6 202 w_xAccumulator += gyroscope.getGyroX();
Strikewolf 1:dacf7db790f6 203 w_yAccumulator += gyroscope.getGyroY();
Strikewolf 1:dacf7db790f6 204 w_zAccumulator += gyroscope.getGyroZ();
Strikewolf 1:dacf7db790f6 205 wait(GYRO_RATE);
Strikewolf 1:dacf7db790f6 206
Strikewolf 0:84d5aa80fd77 207 }
Strikewolf 1:dacf7db790f6 208
Strikewolf 1:dacf7db790f6 209 //Average the samples.
Strikewolf 1:dacf7db790f6 210 w_xAccumulator /= CALIBRATION_SAMPLES;
Strikewolf 1:dacf7db790f6 211 w_yAccumulator /= CALIBRATION_SAMPLES;
Strikewolf 1:dacf7db790f6 212 w_zAccumulator /= CALIBRATION_SAMPLES;
Strikewolf 1:dacf7db790f6 213
Strikewolf 1:dacf7db790f6 214 w_xBias = w_xAccumulator;
Strikewolf 1:dacf7db790f6 215 w_yBias = w_yAccumulator;
Strikewolf 1:dacf7db790f6 216 w_zBias = w_zAccumulator;
Strikewolf 1:dacf7db790f6 217
Strikewolf 1:dacf7db790f6 218 w_xAccumulator = 0;
Strikewolf 1:dacf7db790f6 219 w_yAccumulator = 0;
Strikewolf 1:dacf7db790f6 220 w_zAccumulator = 0;
Strikewolf 1:dacf7db790f6 221
Strikewolf 1:dacf7db790f6 222 }
Strikewolf 1:dacf7db790f6 223
Strikewolf 1:dacf7db790f6 224 void sampleGyroscope(void)
Strikewolf 1:dacf7db790f6 225 {
Strikewolf 1:dacf7db790f6 226
Strikewolf 1:dacf7db790f6 227 //Have we taken enough samples?
Strikewolf 1:dacf7db790f6 228 if (gyroscopeSamples == SAMPLES) {
Strikewolf 1:dacf7db790f6 229
Strikewolf 1:dacf7db790f6 230 //Average the samples, remove the bias, and calculate the angular
Strikewolf 1:dacf7db790f6 231 //velocity in rad/s.
Strikewolf 1:dacf7db790f6 232 w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
Strikewolf 1:dacf7db790f6 233 w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
Strikewolf 1:dacf7db790f6 234 w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
Strikewolf 1:dacf7db790f6 235
Strikewolf 0:84d5aa80fd77 236 w_xAccumulator = 0;
Strikewolf 0:84d5aa80fd77 237 w_yAccumulator = 0;
Strikewolf 0:84d5aa80fd77 238 w_zAccumulator = 0;
Strikewolf 1:dacf7db790f6 239 gyroscopeSamples = 0;
Strikewolf 1:dacf7db790f6 240
Strikewolf 1:dacf7db790f6 241 } else {
Strikewolf 1:dacf7db790f6 242 //Take another sample.
Strikewolf 1:dacf7db790f6 243 w_xAccumulator += gyroscope.getGyroX();
Strikewolf 1:dacf7db790f6 244 w_yAccumulator += gyroscope.getGyroY();
Strikewolf 1:dacf7db790f6 245 w_zAccumulator += gyroscope.getGyroZ();
Strikewolf 1:dacf7db790f6 246
Strikewolf 1:dacf7db790f6 247 gyroscopeSamples++;
Strikewolf 1:dacf7db790f6 248
Strikewolf 0:84d5aa80fd77 249 }
Strikewolf 1:dacf7db790f6 250
Strikewolf 1:dacf7db790f6 251 }
Strikewolf 1:dacf7db790f6 252
Strikewolf 1:dacf7db790f6 253 void filter(void)
Strikewolf 1:dacf7db790f6 254 {
Strikewolf 1:dacf7db790f6 255
Strikewolf 1:dacf7db790f6 256 //Update the filter variables.
Strikewolf 1:dacf7db790f6 257 imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
Strikewolf 1:dacf7db790f6 258 //Calculate the new Euler angles.
Strikewolf 1:dacf7db790f6 259 imuFilter.computeEuler();
Strikewolf 1:dacf7db790f6 260
Strikewolf 1:dacf7db790f6 261 }
Strikewolf 1:dacf7db790f6 262
Strikewolf 1:dacf7db790f6 263 void rpy_init()
Strikewolf 1:dacf7db790f6 264 {
Strikewolf 1:dacf7db790f6 265 //Initialize inertial sensors.
Strikewolf 1:dacf7db790f6 266 initializeAccelerometer();
Strikewolf 1:dacf7db790f6 267 calibrateAccelerometer();
Strikewolf 1:dacf7db790f6 268 initializeGyroscope();
Strikewolf 1:dacf7db790f6 269 calibrateGyroscope();
Strikewolf 1:dacf7db790f6 270
Strikewolf 1:dacf7db790f6 271
Strikewolf 1:dacf7db790f6 272 //Set up timers.
Strikewolf 1:dacf7db790f6 273 //Accelerometer data rate is 200Hz, so we'll sample at this speed.
Strikewolf 1:dacf7db790f6 274 accelerometerTicker.attach(&sampleAccelerometer, 0.005);
Strikewolf 1:dacf7db790f6 275 //Gyroscope data rate is 200Hz, so we'll sample at this speed.
Strikewolf 1:dacf7db790f6 276 gyroscopeTicker.attach(&sampleGyroscope, 0.005);
Strikewolf 1:dacf7db790f6 277 //Update the filter variables at the correct rate.
Strikewolf 1:dacf7db790f6 278 filterTicker.attach(&filter, FILTER_RATE);
Strikewolf 1:dacf7db790f6 279 }