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?
User | Revision | Line number | New 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 */ |