Justin Eng / Mbed 2 deprecated RoboticsCatAndMouse_HumanDriver

Spring 2014, ECE 4180 project, Georgia Institute of Technolgoy. This is the human driver (RF controller) program for the Robotics Cat and Mouse program.

Dependencies:   ADXL345_I2C_NEST HMC6352 IMUfilter ITG3200_NEST USBHost mbed-rtos mbed

Fork of Project by Ganesh Subramaniam

IMU_RPY.h@6:3fb9f96765f6, 2014-04-30 (annotated)

Committer:
Strikewolf
Date:
Wed Apr 30 12:23:04 2014 +0000
Revision:
6:3fb9f96765f6
Parent:
5:210cd333f770 
Final product before demo

Who changed what in which revision?

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