An advanced robot that uses PID control on the motor speed, and an IMU for making accurate turns.

Dependencies:   mbed Motor ITG3200 QEI ADXL345 IMUfilter PID

Committer:
aberk
Date:
Fri Sep 10 14:03:00 2010 +0000
Revision:
0:7440a03255a7
Version 1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aberk 0:7440a03255a7 1 /**
aberk 0:7440a03255a7 2 * @author Aaron Berk
aberk 0:7440a03255a7 3 *
aberk 0:7440a03255a7 4 * @section LICENSE
aberk 0:7440a03255a7 5 *
aberk 0:7440a03255a7 6 * Copyright (c) 2010 ARM Limited
aberk 0:7440a03255a7 7 *
aberk 0:7440a03255a7 8 * Permission is hereby granted, free of charge, to any person obtaining a copy
aberk 0:7440a03255a7 9 * of this software and associated documentation files (the "Software"), to deal
aberk 0:7440a03255a7 10 * in the Software without restriction, including without limitation the rights
aberk 0:7440a03255a7 11 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
aberk 0:7440a03255a7 12 * copies of the Software, and to permit persons to whom the Software is
aberk 0:7440a03255a7 13 * furnished to do so, subject to the following conditions:
aberk 0:7440a03255a7 14 *
aberk 0:7440a03255a7 15 * The above copyright notice and this permission notice shall be included in
aberk 0:7440a03255a7 16 * all copies or substantial portions of the Software.
aberk 0:7440a03255a7 17 *
aberk 0:7440a03255a7 18 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
aberk 0:7440a03255a7 19 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
aberk 0:7440a03255a7 20 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
aberk 0:7440a03255a7 21 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
aberk 0:7440a03255a7 22 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
aberk 0:7440a03255a7 23 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
aberk 0:7440a03255a7 24 * THE SOFTWARE.
aberk 0:7440a03255a7 25 *
aberk 0:7440a03255a7 26 * @section DESCRIPTION
aberk 0:7440a03255a7 27 *
aberk 0:7440a03255a7 28 * IMU consisting of ADXL345 accelerometer and ITG-3200 gyroscope using
aberk 0:7440a03255a7 29 * orientation filter developed by Sebastian Madgwick.
aberk 0:7440a03255a7 30 *
aberk 0:7440a03255a7 31 * Find more details about his paper here:
aberk 0:7440a03255a7 32 *
aberk 0:7440a03255a7 33 * http://code.google.com/p/imumargalgorithm30042010sohm/
aberk 0:7440a03255a7 34 */
aberk 0:7440a03255a7 35
aberk 0:7440a03255a7 36
aberk 0:7440a03255a7 37 /**
aberk 0:7440a03255a7 38 * Includes
aberk 0:7440a03255a7 39 */
aberk 0:7440a03255a7 40 #include "IMU.h"
aberk 0:7440a03255a7 41
aberk 0:7440a03255a7 42 IMU::IMU(float imuRate,
aberk 0:7440a03255a7 43 double gyroscopeMeasurementError,
aberk 0:7440a03255a7 44 float accelerometerRate,
aberk 0:7440a03255a7 45 float gyroscopeRate) : accelerometer(p5, p6, p7, p8),
aberk 0:7440a03255a7 46 gyroscope(p9, p10), imuFilter(imuRate, gyroscopeMeasurementError) {
aberk 0:7440a03255a7 47
aberk 0:7440a03255a7 48 imuRate_ = imuRate;
aberk 0:7440a03255a7 49 accelerometerRate_ = accelerometerRate;
aberk 0:7440a03255a7 50 gyroscopeRate_ = gyroscopeRate;
aberk 0:7440a03255a7 51
aberk 0:7440a03255a7 52 //Initialize sampling variables.
aberk 0:7440a03255a7 53 a_xAccumulator = 0;
aberk 0:7440a03255a7 54 a_yAccumulator = 0;
aberk 0:7440a03255a7 55 a_zAccumulator = 0;
aberk 0:7440a03255a7 56 w_xAccumulator = 0;
aberk 0:7440a03255a7 57 w_yAccumulator = 0;
aberk 0:7440a03255a7 58 w_zAccumulator = 0;
aberk 0:7440a03255a7 59
aberk 0:7440a03255a7 60 accelerometerSamples = 0;
aberk 0:7440a03255a7 61 gyroscopeSamples = 0;
aberk 0:7440a03255a7 62
aberk 0:7440a03255a7 63 //Initialize and calibrate sensors.
aberk 0:7440a03255a7 64 initializeAccelerometer();
aberk 0:7440a03255a7 65 calibrateAccelerometer();
aberk 0:7440a03255a7 66
aberk 0:7440a03255a7 67 initializeGyroscope();
aberk 0:7440a03255a7 68 calibrateGyroscope();
aberk 0:7440a03255a7 69
aberk 0:7440a03255a7 70 //To reduce the number of interrupts we'll remove the separate tickers for
aberk 0:7440a03255a7 71 //the accelerometer, gyro and filter update and combine them all into one.
aberk 0:7440a03255a7 72
aberk 0:7440a03255a7 73 //accelerometerTicker.attach(this, &IMU::sampleAccelerometer, accelerometerRate_);
aberk 0:7440a03255a7 74 //gyroscopeTicker.attach(this, &IMU::sampleGyroscope, gyroscopeRate_);
aberk 0:7440a03255a7 75 sampleTicker.attach(this, &IMU::sample, accelerometerRate_);
aberk 0:7440a03255a7 76 //filterTicker.attach(this, &IMU::filter, imuRate_);
aberk 0:7440a03255a7 77
aberk 0:7440a03255a7 78 }
aberk 0:7440a03255a7 79
aberk 0:7440a03255a7 80 double IMU::getRoll(void) {
aberk 0:7440a03255a7 81
aberk 0:7440a03255a7 82 return toDegrees(imuFilter.getRoll());
aberk 0:7440a03255a7 83
aberk 0:7440a03255a7 84 }
aberk 0:7440a03255a7 85
aberk 0:7440a03255a7 86 double IMU::getPitch(void) {
aberk 0:7440a03255a7 87
aberk 0:7440a03255a7 88 return toDegrees(imuFilter.getPitch());
aberk 0:7440a03255a7 89
aberk 0:7440a03255a7 90 }
aberk 0:7440a03255a7 91
aberk 0:7440a03255a7 92 double IMU::getYaw(void) {
aberk 0:7440a03255a7 93
aberk 0:7440a03255a7 94 return toDegrees(imuFilter.getYaw());
aberk 0:7440a03255a7 95
aberk 0:7440a03255a7 96 }
aberk 0:7440a03255a7 97
aberk 0:7440a03255a7 98 void IMU::initializeAccelerometer(void) {
aberk 0:7440a03255a7 99
aberk 0:7440a03255a7 100 //Go into standby mode to configure the device.
aberk 0:7440a03255a7 101 accelerometer.setPowerControl(0x00);
aberk 0:7440a03255a7 102 //Full resolution, +/-16g, 4mg/LSB.
aberk 0:7440a03255a7 103 accelerometer.setDataFormatControl(0x0B);
aberk 0:7440a03255a7 104 //200Hz data rate.
aberk 0:7440a03255a7 105 accelerometer.setDataRate(ADXL345_200HZ);
aberk 0:7440a03255a7 106 //Measurement mode.
aberk 0:7440a03255a7 107 accelerometer.setPowerControl(0x08);
aberk 0:7440a03255a7 108 //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
aberk 0:7440a03255a7 109 wait_ms(22);
aberk 0:7440a03255a7 110
aberk 0:7440a03255a7 111 }
aberk 0:7440a03255a7 112
aberk 0:7440a03255a7 113 void IMU::sampleAccelerometer(void) {
aberk 0:7440a03255a7 114
aberk 0:7440a03255a7 115 //If we've taken a certain number of samples,
aberk 0:7440a03255a7 116 //average them, remove the bias and convert the units.
aberk 0:7440a03255a7 117 if (accelerometerSamples == SAMPLES) {
aberk 0:7440a03255a7 118
aberk 0:7440a03255a7 119 a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
aberk 0:7440a03255a7 120 a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
aberk 0:7440a03255a7 121 a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
aberk 0:7440a03255a7 122
aberk 0:7440a03255a7 123 a_xAccumulator = 0;
aberk 0:7440a03255a7 124 a_yAccumulator = 0;
aberk 0:7440a03255a7 125 a_zAccumulator = 0;
aberk 0:7440a03255a7 126 accelerometerSamples = 0;
aberk 0:7440a03255a7 127
aberk 0:7440a03255a7 128 }
aberk 0:7440a03255a7 129 //Otherwise, accumulate another reading.
aberk 0:7440a03255a7 130 else {
aberk 0:7440a03255a7 131
aberk 0:7440a03255a7 132 accelerometer.getOutput(readings);
aberk 0:7440a03255a7 133
aberk 0:7440a03255a7 134 a_xAccumulator += (int16_t) readings[0];
aberk 0:7440a03255a7 135 a_yAccumulator += (int16_t) readings[1];
aberk 0:7440a03255a7 136 a_zAccumulator += (int16_t) readings[2];
aberk 0:7440a03255a7 137
aberk 0:7440a03255a7 138 accelerometerSamples++;
aberk 0:7440a03255a7 139
aberk 0:7440a03255a7 140 }
aberk 0:7440a03255a7 141
aberk 0:7440a03255a7 142 }
aberk 0:7440a03255a7 143
aberk 0:7440a03255a7 144 void IMU::calibrateAccelerometer(void) {
aberk 0:7440a03255a7 145
aberk 0:7440a03255a7 146 a_xAccumulator = 0;
aberk 0:7440a03255a7 147 a_yAccumulator = 0;
aberk 0:7440a03255a7 148 a_zAccumulator = 0;
aberk 0:7440a03255a7 149
aberk 0:7440a03255a7 150 //Accumulate a certain number of samples.
aberk 0:7440a03255a7 151 for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
aberk 0:7440a03255a7 152
aberk 0:7440a03255a7 153 accelerometer.getOutput(readings);
aberk 0:7440a03255a7 154
aberk 0:7440a03255a7 155 a_xAccumulator += (int16_t) readings[0];
aberk 0:7440a03255a7 156 a_yAccumulator += (int16_t) readings[1];
aberk 0:7440a03255a7 157 a_zAccumulator += (int16_t) readings[2];
aberk 0:7440a03255a7 158
aberk 0:7440a03255a7 159 wait(accelerometerRate_);
aberk 0:7440a03255a7 160
aberk 0:7440a03255a7 161 }
aberk 0:7440a03255a7 162
aberk 0:7440a03255a7 163 //Average the samples.
aberk 0:7440a03255a7 164 a_xAccumulator /= CALIBRATION_SAMPLES;
aberk 0:7440a03255a7 165 a_yAccumulator /= CALIBRATION_SAMPLES;
aberk 0:7440a03255a7 166 a_zAccumulator /= CALIBRATION_SAMPLES;
aberk 0:7440a03255a7 167
aberk 0:7440a03255a7 168 //These are our zero g offsets.
aberk 0:7440a03255a7 169 //250 = 9.81m/s/s @ 4mg/LSB.
aberk 0:7440a03255a7 170 a_xBias = a_xAccumulator;
aberk 0:7440a03255a7 171 a_yBias = a_yAccumulator;
aberk 0:7440a03255a7 172 a_zBias = (a_zAccumulator - 250);
aberk 0:7440a03255a7 173
aberk 0:7440a03255a7 174 //Reset accumulators.
aberk 0:7440a03255a7 175 a_xAccumulator = 0;
aberk 0:7440a03255a7 176 a_yAccumulator = 0;
aberk 0:7440a03255a7 177 a_zAccumulator = 0;
aberk 0:7440a03255a7 178
aberk 0:7440a03255a7 179 }
aberk 0:7440a03255a7 180
aberk 0:7440a03255a7 181 void IMU::initializeGyroscope(void) {
aberk 0:7440a03255a7 182
aberk 0:7440a03255a7 183 //Low pass filter bandwidth of 42Hz.
aberk 0:7440a03255a7 184 gyroscope.setLpBandwidth(LPFBW_42HZ);
aberk 0:7440a03255a7 185 //Internal sample rate of 200Hz.
aberk 0:7440a03255a7 186 gyroscope.setSampleRateDivider(4);
aberk 0:7440a03255a7 187
aberk 0:7440a03255a7 188 }
aberk 0:7440a03255a7 189
aberk 0:7440a03255a7 190 void IMU::calibrateGyroscope(void) {
aberk 0:7440a03255a7 191
aberk 0:7440a03255a7 192 w_xAccumulator = 0;
aberk 0:7440a03255a7 193 w_yAccumulator = 0;
aberk 0:7440a03255a7 194 w_zAccumulator = 0;
aberk 0:7440a03255a7 195
aberk 0:7440a03255a7 196 //Accumulate a certain number of samples.
aberk 0:7440a03255a7 197 for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
aberk 0:7440a03255a7 198
aberk 0:7440a03255a7 199 w_xAccumulator += gyroscope.getGyroX();
aberk 0:7440a03255a7 200 w_yAccumulator += gyroscope.getGyroY();
aberk 0:7440a03255a7 201 w_zAccumulator += gyroscope.getGyroZ();
aberk 0:7440a03255a7 202 wait(gyroscopeRate_);
aberk 0:7440a03255a7 203
aberk 0:7440a03255a7 204 }
aberk 0:7440a03255a7 205
aberk 0:7440a03255a7 206 //Average the samples.
aberk 0:7440a03255a7 207 w_xAccumulator /= CALIBRATION_SAMPLES;
aberk 0:7440a03255a7 208 w_yAccumulator /= CALIBRATION_SAMPLES;
aberk 0:7440a03255a7 209 w_zAccumulator /= CALIBRATION_SAMPLES;
aberk 0:7440a03255a7 210
aberk 0:7440a03255a7 211 //Set the null bias.
aberk 0:7440a03255a7 212 w_xBias = w_xAccumulator;
aberk 0:7440a03255a7 213 w_yBias = w_yAccumulator;
aberk 0:7440a03255a7 214 w_zBias = w_zAccumulator;
aberk 0:7440a03255a7 215
aberk 0:7440a03255a7 216 //Reset the accumulators.
aberk 0:7440a03255a7 217 w_xAccumulator = 0;
aberk 0:7440a03255a7 218 w_yAccumulator = 0;
aberk 0:7440a03255a7 219 w_zAccumulator = 0;
aberk 0:7440a03255a7 220
aberk 0:7440a03255a7 221 }
aberk 0:7440a03255a7 222
aberk 0:7440a03255a7 223 void IMU::sampleGyroscope(void) {
aberk 0:7440a03255a7 224
aberk 0:7440a03255a7 225 //If we've taken the required number of samples then,
aberk 0:7440a03255a7 226 //average the samples, removed the null bias and convert the units
aberk 0:7440a03255a7 227 //to rad/s.
aberk 0:7440a03255a7 228 if (gyroscopeSamples == SAMPLES) {
aberk 0:7440a03255a7 229
aberk 0:7440a03255a7 230 w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
aberk 0:7440a03255a7 231 w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
aberk 0:7440a03255a7 232 w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
aberk 0:7440a03255a7 233
aberk 0:7440a03255a7 234 w_xAccumulator = 0;
aberk 0:7440a03255a7 235 w_yAccumulator = 0;
aberk 0:7440a03255a7 236 w_zAccumulator = 0;
aberk 0:7440a03255a7 237 gyroscopeSamples = 0;
aberk 0:7440a03255a7 238
aberk 0:7440a03255a7 239 }
aberk 0:7440a03255a7 240 //Accumulate another sample.
aberk 0:7440a03255a7 241 else {
aberk 0:7440a03255a7 242
aberk 0:7440a03255a7 243 w_xAccumulator += gyroscope.getGyroX();
aberk 0:7440a03255a7 244 w_yAccumulator += gyroscope.getGyroY();
aberk 0:7440a03255a7 245 w_zAccumulator += gyroscope.getGyroZ();
aberk 0:7440a03255a7 246
aberk 0:7440a03255a7 247 gyroscopeSamples++;
aberk 0:7440a03255a7 248
aberk 0:7440a03255a7 249 }
aberk 0:7440a03255a7 250
aberk 0:7440a03255a7 251 }
aberk 0:7440a03255a7 252
aberk 0:7440a03255a7 253 void IMU::sample(void) {
aberk 0:7440a03255a7 254
aberk 0:7440a03255a7 255 //If we've taken enough samples then,
aberk 0:7440a03255a7 256 //average the samples, remove the offsets and convert to appropriate units.
aberk 0:7440a03255a7 257 //Feed this information into the filter to calculate the new Euler angles.
aberk 0:7440a03255a7 258 if (accelerometerSamples == SAMPLES) {
aberk 0:7440a03255a7 259
aberk 0:7440a03255a7 260 a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
aberk 0:7440a03255a7 261 a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
aberk 0:7440a03255a7 262 a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
aberk 0:7440a03255a7 263
aberk 0:7440a03255a7 264 a_xAccumulator = 0;
aberk 0:7440a03255a7 265 a_yAccumulator = 0;
aberk 0:7440a03255a7 266 a_zAccumulator = 0;
aberk 0:7440a03255a7 267
aberk 0:7440a03255a7 268 accelerometerSamples = 0;
aberk 0:7440a03255a7 269
aberk 0:7440a03255a7 270 w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
aberk 0:7440a03255a7 271 w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
aberk 0:7440a03255a7 272 w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
aberk 0:7440a03255a7 273
aberk 0:7440a03255a7 274 w_xAccumulator = 0;
aberk 0:7440a03255a7 275 w_yAccumulator = 0;
aberk 0:7440a03255a7 276 w_zAccumulator = 0;
aberk 0:7440a03255a7 277 gyroscopeSamples = 0;
aberk 0:7440a03255a7 278
aberk 0:7440a03255a7 279 //Update the filter variables.
aberk 0:7440a03255a7 280 imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
aberk 0:7440a03255a7 281 //Calculate the new Euler angles.
aberk 0:7440a03255a7 282 imuFilter.computeEuler();
aberk 0:7440a03255a7 283
aberk 0:7440a03255a7 284 }
aberk 0:7440a03255a7 285 //Accumulate another sample.
aberk 0:7440a03255a7 286 else {
aberk 0:7440a03255a7 287
aberk 0:7440a03255a7 288 accelerometer.getOutput(readings);
aberk 0:7440a03255a7 289
aberk 0:7440a03255a7 290 a_xAccumulator += (int16_t) readings[0];
aberk 0:7440a03255a7 291 a_yAccumulator += (int16_t) readings[1];
aberk 0:7440a03255a7 292 a_zAccumulator += (int16_t) readings[2];
aberk 0:7440a03255a7 293
aberk 0:7440a03255a7 294 w_xAccumulator += gyroscope.getGyroX();
aberk 0:7440a03255a7 295 w_yAccumulator += gyroscope.getGyroY();
aberk 0:7440a03255a7 296 w_zAccumulator += gyroscope.getGyroZ();
aberk 0:7440a03255a7 297
aberk 0:7440a03255a7 298 accelerometerSamples++;
aberk 0:7440a03255a7 299
aberk 0:7440a03255a7 300 }
aberk 0:7440a03255a7 301
aberk 0:7440a03255a7 302 }
aberk 0:7440a03255a7 303
aberk 0:7440a03255a7 304 void IMU::filter(void) {
aberk 0:7440a03255a7 305
aberk 0:7440a03255a7 306 //Update the filter variables.
aberk 0:7440a03255a7 307 imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
aberk 0:7440a03255a7 308 //Calculate the new Euler angles.
aberk 0:7440a03255a7 309 imuFilter.computeEuler();
aberk 0:7440a03255a7 310
aberk 0:7440a03255a7 311 }
aberk 0:7440a03255a7 312
aberk 0:7440a03255a7 313 void IMU::reset(void) {
aberk 0:7440a03255a7 314
aberk 0:7440a03255a7 315 //Disable interrupts.
aberk 0:7440a03255a7 316 sampleTicker.detach();
aberk 0:7440a03255a7 317
aberk 0:7440a03255a7 318 //Recalibrate sensors.
aberk 0:7440a03255a7 319 calibrateAccelerometer();
aberk 0:7440a03255a7 320 calibrateGyroscope();
aberk 0:7440a03255a7 321
aberk 0:7440a03255a7 322 //Reset the IMU filter.
aberk 0:7440a03255a7 323 //imuFilter.reset();
aberk 0:7440a03255a7 324
aberk 0:7440a03255a7 325 //Reset the working variables.
aberk 0:7440a03255a7 326 a_xAccumulator = 0;
aberk 0:7440a03255a7 327 a_yAccumulator = 0;
aberk 0:7440a03255a7 328 a_zAccumulator = 0;
aberk 0:7440a03255a7 329 w_xAccumulator = 0;
aberk 0:7440a03255a7 330 w_yAccumulator = 0;
aberk 0:7440a03255a7 331 w_zAccumulator = 0;
aberk 0:7440a03255a7 332 accelerometerSamples = 0;
aberk 0:7440a03255a7 333 gyroscopeSamples = 0;
aberk 0:7440a03255a7 334
aberk 0:7440a03255a7 335 //Enable interrupts.
aberk 0:7440a03255a7 336 sampleTicker.attach(this, &IMU::sample, accelerometerRate_);
aberk 0:7440a03255a7 337
aberk 0:7440a03255a7 338 }