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@0:84d5aa80fd77, 2014-04-27 (annotated)

Committer:
Strikewolf
Date:
Sun Apr 27 04:31:07 2014 +0000
Revision:
0:84d5aa80fd77
Child:
1:dacf7db790f6
-v1.0 of human controlled car;

Who changed what in which revision?

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