Aaron Berk / Mbed 2 deprecated IMURover

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

IMU.h@0:7440a03255a7, 2010-09-10 (annotated)

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 #ifndef MBED_IMU_H
aberk 0:7440a03255a7 37 #define MBED_IMU_H
aberk 0:7440a03255a7 38
aberk 0:7440a03255a7 39 /**
aberk 0:7440a03255a7 40 * Includes
aberk 0:7440a03255a7 41 */
aberk 0:7440a03255a7 42 #include "mbed.h"
aberk 0:7440a03255a7 43 #include "ADXL345.h"
aberk 0:7440a03255a7 44 #include "ITG3200.h"
aberk 0:7440a03255a7 45 #include "IMUfilter.h"
aberk 0:7440a03255a7 46
aberk 0:7440a03255a7 47 /**
aberk 0:7440a03255a7 48 * Defines
aberk 0:7440a03255a7 49 */
aberk 0:7440a03255a7 50 #define IMU_RATE 0.025
aberk 0:7440a03255a7 51 #define ACCELEROMETER_RATE 0.005
aberk 0:7440a03255a7 52 #define GYROSCOPE_RATE 0.005
aberk 0:7440a03255a7 53 #define GYRO_MEAS_ERROR 0.3 //IMUfilter tuning parameter.
aberk 0:7440a03255a7 54
aberk 0:7440a03255a7 55 //Gravity at Earth's surface in m/s/s
aberk 0:7440a03255a7 56 #define g0 9.812865328
aberk 0:7440a03255a7 57 //Number of samples to average
aberk 0:7440a03255a7 58 #define SAMPLES 4
aberk 0:7440a03255a7 59 #define CALIBRATION_SAMPLES 128
aberk 0:7440a03255a7 60 //Multiply radians to get degrees.
aberk 0:7440a03255a7 61 #define toDegrees(x) (x * 57.2957795)
aberk 0:7440a03255a7 62 //Multiply degrees to get radians.
aberk 0:7440a03255a7 63 #define toRadians(x) (x * 0.01745329252)
aberk 0:7440a03255a7 64 //Full scale resolution on the ADXL345 is 4mg/LSB.
aberk 0:7440a03255a7 65 //Multiply ADC count readings from ADXL345 to get acceleration in m/s/s.
aberk 0:7440a03255a7 66 #define toAcceleration(x) (x * (4 * g0 * 0.001))
aberk 0:7440a03255a7 67 //14.375 LSB/(degrees/sec)
aberk 0:7440a03255a7 68 #define GYROSCOPE_GAIN (1 / 14.375)
aberk 0:7440a03255a7 69 #define ACCELEROMETER_GAIN (0.004 * g0)
aberk 0:7440a03255a7 70
aberk 0:7440a03255a7 71 /**
aberk 0:7440a03255a7 72 * IMU consisting of ADXL345 accelerometer and ITG-3200 gyroscope to calculate
aberk 0:7440a03255a7 73 * roll, pitch and yaw angles.
aberk 0:7440a03255a7 74 */
aberk 0:7440a03255a7 75 class IMU {
aberk 0:7440a03255a7 76
aberk 0:7440a03255a7 77 public:
aberk 0:7440a03255a7 78
aberk 0:7440a03255a7 79 /**
aberk 0:7440a03255a7 80 * Constructor.
aberk 0:7440a03255a7 81 *
aberk 0:7440a03255a7 82 * @param imuRate Rate which IMUfilter update and Euler angle calculation
aberk 0:7440a03255a7 83 * occurs.
aberk 0:7440a03255a7 84 * @param gyroscopeMeasurementError IMUfilter tuning parameter.
aberk 0:7440a03255a7 85 * @param accelerometerRate Rate at which accelerometer data is sampled.
aberk 0:7440a03255a7 86 * @param gyroscopeRate Rate at which gyroscope data is sampled.
aberk 0:7440a03255a7 87 */
aberk 0:7440a03255a7 88 IMU(float imuRate,
aberk 0:7440a03255a7 89 double gyroscopeMeasurementError,
aberk 0:7440a03255a7 90 float accelerometerRate,
aberk 0:7440a03255a7 91 float gyroscopeRate);
aberk 0:7440a03255a7 92
aberk 0:7440a03255a7 93 /**
aberk 0:7440a03255a7 94 * Get the current roll angle.
aberk 0:7440a03255a7 95 *
aberk 0:7440a03255a7 96 * @return The current roll angle in degrees.
aberk 0:7440a03255a7 97 */
aberk 0:7440a03255a7 98 double getRoll(void);
aberk 0:7440a03255a7 99
aberk 0:7440a03255a7 100 /**
aberk 0:7440a03255a7 101 * Get the current pitch angle.
aberk 0:7440a03255a7 102 *
aberk 0:7440a03255a7 103 * @return The current pitch angle in degrees.
aberk 0:7440a03255a7 104 */
aberk 0:7440a03255a7 105 double getPitch(void);
aberk 0:7440a03255a7 106
aberk 0:7440a03255a7 107 /**
aberk 0:7440a03255a7 108 * Get the current yaw angle.
aberk 0:7440a03255a7 109 *
aberk 0:7440a03255a7 110 * @return The current yaw angle in degrees.
aberk 0:7440a03255a7 111 */
aberk 0:7440a03255a7 112 double getYaw(void);
aberk 0:7440a03255a7 113
aberk 0:7440a03255a7 114 /**
aberk 0:7440a03255a7 115 * Sample the sensors, and if enough samples have been taken,
aberk 0:7440a03255a7 116 * take an average, and compute the new Euler angles.
aberk 0:7440a03255a7 117 */
aberk 0:7440a03255a7 118 void sample(void);
aberk 0:7440a03255a7 119
aberk 0:7440a03255a7 120 /**
aberk 0:7440a03255a7 121 * Recalibrate the sensors and reset the IMU filter.
aberk 0:7440a03255a7 122 */
aberk 0:7440a03255a7 123 void reset(void);
aberk 0:7440a03255a7 124
aberk 0:7440a03255a7 125 private:
aberk 0:7440a03255a7 126
aberk 0:7440a03255a7 127 /**
aberk 0:7440a03255a7 128 * Set up the ADXL345 appropriately.
aberk 0:7440a03255a7 129 */
aberk 0:7440a03255a7 130 void initializeAccelerometer(void);
aberk 0:7440a03255a7 131
aberk 0:7440a03255a7 132 /**
aberk 0:7440a03255a7 133 * Calculate the zero g offset.
aberk 0:7440a03255a7 134 */
aberk 0:7440a03255a7 135 void calibrateAccelerometer(void);
aberk 0:7440a03255a7 136
aberk 0:7440a03255a7 137 /**
aberk 0:7440a03255a7 138 * Take a set of samples and average them.
aberk 0:7440a03255a7 139 */
aberk 0:7440a03255a7 140 void sampleAccelerometer(void);
aberk 0:7440a03255a7 141
aberk 0:7440a03255a7 142 /**
aberk 0:7440a03255a7 143 * Set up the ITG-3200 appropriately.
aberk 0:7440a03255a7 144 */
aberk 0:7440a03255a7 145 void initializeGyroscope(void);
aberk 0:7440a03255a7 146
aberk 0:7440a03255a7 147 /**
aberk 0:7440a03255a7 148 * Calculate the bias offset.
aberk 0:7440a03255a7 149 */
aberk 0:7440a03255a7 150 void calibrateGyroscope(void);
aberk 0:7440a03255a7 151
aberk 0:7440a03255a7 152 /**
aberk 0:7440a03255a7 153 * Take a set of samples and average them.
aberk 0:7440a03255a7 154 */
aberk 0:7440a03255a7 155 void sampleGyroscope(void);
aberk 0:7440a03255a7 156
aberk 0:7440a03255a7 157 /**
aberk 0:7440a03255a7 158 * Update the filter and calculate the Euler angles.
aberk 0:7440a03255a7 159 */
aberk 0:7440a03255a7 160 void filter(void);
aberk 0:7440a03255a7 161
aberk 0:7440a03255a7 162 ADXL345 accelerometer;
aberk 0:7440a03255a7 163 ITG3200 gyroscope;
aberk 0:7440a03255a7 164 IMUfilter imuFilter;
aberk 0:7440a03255a7 165
aberk 0:7440a03255a7 166 Ticker accelerometerTicker;
aberk 0:7440a03255a7 167 Ticker gyroscopeTicker;
aberk 0:7440a03255a7 168 Ticker sampleTicker;
aberk 0:7440a03255a7 169 Ticker filterTicker;
aberk 0:7440a03255a7 170
aberk 0:7440a03255a7 171 float accelerometerRate_;
aberk 0:7440a03255a7 172 float gyroscopeRate_;
aberk 0:7440a03255a7 173 float imuRate_;
aberk 0:7440a03255a7 174
aberk 0:7440a03255a7 175 //Offsets for the gyroscope.
aberk 0:7440a03255a7 176 //The readings we take when the gyroscope is stationary won't be 0, so we'll
aberk 0:7440a03255a7 177 //average a set of readings we do get when the gyroscope is stationary and
aberk 0:7440a03255a7 178 //take those away from subsequent readings to ensure the gyroscope is offset
aberk 0:7440a03255a7 179 //or biased to 0.
aberk 0:7440a03255a7 180 double w_xBias;
aberk 0:7440a03255a7 181 double w_yBias;
aberk 0:7440a03255a7 182 double w_zBias;
aberk 0:7440a03255a7 183
aberk 0:7440a03255a7 184 double a_xBias;
aberk 0:7440a03255a7 185 double a_yBias;
aberk 0:7440a03255a7 186 double a_zBias;
aberk 0:7440a03255a7 187
aberk 0:7440a03255a7 188 volatile double a_xAccumulator;
aberk 0:7440a03255a7 189 volatile double a_yAccumulator;
aberk 0:7440a03255a7 190 volatile double a_zAccumulator;
aberk 0:7440a03255a7 191 volatile double w_xAccumulator;
aberk 0:7440a03255a7 192 volatile double w_yAccumulator;
aberk 0:7440a03255a7 193 volatile double w_zAccumulator;
aberk 0:7440a03255a7 194
aberk 0:7440a03255a7 195 //Accelerometer and gyroscope readings for x, y, z axes.
aberk 0:7440a03255a7 196 volatile double a_x;
aberk 0:7440a03255a7 197 volatile double a_y;
aberk 0:7440a03255a7 198 volatile double a_z;
aberk 0:7440a03255a7 199 volatile double w_x;
aberk 0:7440a03255a7 200 volatile double w_y;
aberk 0:7440a03255a7 201 volatile double w_z;
aberk 0:7440a03255a7 202
aberk 0:7440a03255a7 203 //Buffer for accelerometer readings.
aberk 0:7440a03255a7 204 int readings[3];
aberk 0:7440a03255a7 205 int accelerometerSamples;
aberk 0:7440a03255a7 206 int gyroscopeSamples;
aberk 0:7440a03255a7 207
aberk 0:7440a03255a7 208 };
aberk 0:7440a03255a7 209
aberk 0:7440a03255a7 210 #endif /* MBED_IMU_H */