Qinchen Gu / Mbed 2 deprecated P4_IMU

Air Mouse Project

Dependencies:   ADXL345_I2C IMUfilter ITG3200_lib USBDevice mbed

Fork of IMU by Tim Marvin

main.cpp@2:044740372a78, 2014-03-24 (annotated)

Committer:
guqinchen
Date:
Mon Mar 24 15:15:37 2014 +0000
Revision:
2:044740372a78
Parent:
1:c8232c909f29 
Air Mouse Project

Who changed what in which revision?

UserRevisionLine numberNew contents of line
atommota 0:a070fa765ed2 1 /**
atommota 0:a070fa765ed2 2 * IMU filter example.
atommota 0:a070fa765ed2 3 *
atommota 0:a070fa765ed2 4 * Calculate the roll, pitch and yaw angles.
atommota 0:a070fa765ed2 5 */
atommota 0:a070fa765ed2 6 #include "IMUfilter.h"
guqinchen 1:c8232c909f29 7 #include "ADXL345_I2C.h"
atommota 0:a070fa765ed2 8 #include "ITG3200.h"
guqinchen 1:c8232c909f29 9 #include "USBMouse.h"
atommota 0:a070fa765ed2 10
guqinchen 1:c8232c909f29 11
guqinchen 1:c8232c909f29 12 #define MOVERATE 1
guqinchen 1:c8232c909f29 13 #define MAXMOVE 100
guqinchen 1:c8232c909f29 14 #define MOVETHRESHOLD 3
atommota 0:a070fa765ed2 15 //Gravity at Earth's surface in m/s/s
atommota 0:a070fa765ed2 16 #define g0 9.812865328
atommota 0:a070fa765ed2 17 //Number of samples to average.
guqinchen 1:c8232c909f29 18 #define SAMPLES 2
atommota 0:a070fa765ed2 19 //Number of samples to be averaged for a null bias calculation
atommota 0:a070fa765ed2 20 //during calibration.
guqinchen 1:c8232c909f29 21 #define CALIBRATION_SAMPLES 32
atommota 0:a070fa765ed2 22 //Convert from radians to degrees.
atommota 0:a070fa765ed2 23 #define toDegrees(x) (x * 57.2957795)
atommota 0:a070fa765ed2 24 //Convert from degrees to radians.
atommota 0:a070fa765ed2 25 #define toRadians(x) (x * 0.01745329252)
atommota 0:a070fa765ed2 26 //ITG-3200 sensitivity is 14.375 LSB/(degrees/sec).
atommota 0:a070fa765ed2 27 #define GYROSCOPE_GAIN (1 / 14.375)
atommota 0:a070fa765ed2 28 //Full scale resolution on the ADXL345 is 4mg/LSB.
atommota 0:a070fa765ed2 29 #define ACCELEROMETER_GAIN (0.000061035 * g0)
guqinchen 1:c8232c909f29 30 //Sampling gyroscope at .
atommota 0:a070fa765ed2 31 #define GYRO_RATE 0.005
guqinchen 1:c8232c909f29 32 //Sampling accelerometer at
atommota 0:a070fa765ed2 33 #define ACC_RATE 0.005
guqinchen 1:c8232c909f29 34 #define FILTER_RATE 0.015
guqinchen 1:c8232c909f29 35
guqinchen 1:c8232c909f29 36 DigitalIn leftClick(p16);
guqinchen 1:c8232c909f29 37 DigitalIn rightClick(p15);
atommota 0:a070fa765ed2 38
atommota 0:a070fa765ed2 39 Serial pc(USBTX, USBRX);
atommota 0:a070fa765ed2 40 //At rest the gyroscope is centred around 0 and goes between about
atommota 0:a070fa765ed2 41 //-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly
atommota 0:a070fa765ed2 42 //5/15 = 0.3 degrees/sec.
atommota 0:a070fa765ed2 43 IMUfilter imuFilter(FILTER_RATE, 0.3);
guqinchen 1:c8232c909f29 44 ADXL345_I2C accelerometer(p28, p27);
guqinchen 1:c8232c909f29 45 ITG3200 gyroscope(p28, p27);
guqinchen 1:c8232c909f29 46 USBMouse mouse;
guqinchen 1:c8232c909f29 47
atommota 0:a070fa765ed2 48 Ticker accelerometerTicker;
atommota 0:a070fa765ed2 49 Ticker gyroscopeTicker;
atommota 0:a070fa765ed2 50 Ticker filterTicker;
atommota 0:a070fa765ed2 51
guqinchen 1:c8232c909f29 52 /**
guqinchen 1:c8232c909f29 53 * IMU filter example.
guqinchen 1:c8232c909f29 54 *
guqinchen 1:c8232c909f29 55 * Calculate the roll, pitch and yaw angles.
guqinchen 1:c8232c909f29 56 */
guqinchen 1:c8232c909f29 57
guqinchen 1:c8232c909f29 58
guqinchen 1:c8232c909f29 59
atommota 0:a070fa765ed2 60 //Offsets for the gyroscope.
atommota 0:a070fa765ed2 61 //The readings we take when the gyroscope is stationary won't be 0, so we'll
atommota 0:a070fa765ed2 62 //average a set of readings we do get when the gyroscope is stationary and
atommota 0:a070fa765ed2 63 //take those away from subsequent readings to ensure the gyroscope is offset
atommota 0:a070fa765ed2 64 //or "biased" to 0.
atommota 0:a070fa765ed2 65 double w_xBias;
atommota 0:a070fa765ed2 66 double w_yBias;
atommota 0:a070fa765ed2 67 double w_zBias;
atommota 0:a070fa765ed2 68
atommota 0:a070fa765ed2 69 //Offsets for the accelerometer.
atommota 0:a070fa765ed2 70 //Same as with the gyroscope.
atommota 0:a070fa765ed2 71 double a_xBias;
atommota 0:a070fa765ed2 72 double a_yBias;
atommota 0:a070fa765ed2 73 double a_zBias;
atommota 0:a070fa765ed2 74
atommota 0:a070fa765ed2 75 //Accumulators used for oversampling and then averaging.
atommota 0:a070fa765ed2 76 volatile double a_xAccumulator = 0;
atommota 0:a070fa765ed2 77 volatile double a_yAccumulator = 0;
atommota 0:a070fa765ed2 78 volatile double a_zAccumulator = 0;
atommota 0:a070fa765ed2 79 volatile double w_xAccumulator = 0;
atommota 0:a070fa765ed2 80 volatile double w_yAccumulator = 0;
atommota 0:a070fa765ed2 81 volatile double w_zAccumulator = 0;
atommota 0:a070fa765ed2 82
atommota 0:a070fa765ed2 83 //Accelerometer and gyroscope readings for x, y, z axes.
atommota 0:a070fa765ed2 84 volatile double a_x;
atommota 0:a070fa765ed2 85 volatile double a_y;
atommota 0:a070fa765ed2 86 volatile double a_z;
atommota 0:a070fa765ed2 87 volatile double w_x;
atommota 0:a070fa765ed2 88 volatile double w_y;
atommota 0:a070fa765ed2 89 volatile double w_z;
atommota 0:a070fa765ed2 90
atommota 0:a070fa765ed2 91 //Buffer for accelerometer readings.
atommota 0:a070fa765ed2 92 int readings[3];
atommota 0:a070fa765ed2 93 //Number of accelerometer samples we're on.
atommota 0:a070fa765ed2 94 int accelerometerSamples = 0;
atommota 0:a070fa765ed2 95 //Number of gyroscope samples we're on.
atommota 0:a070fa765ed2 96 int gyroscopeSamples = 0;
atommota 0:a070fa765ed2 97
atommota 0:a070fa765ed2 98 /**
atommota 0:a070fa765ed2 99 * Prototypes
atommota 0:a070fa765ed2 100 */
atommota 0:a070fa765ed2 101 //Set up the ADXL345 appropriately.
atommota 0:a070fa765ed2 102 void initializeAcceleromter(void);
atommota 0:a070fa765ed2 103 //Calculate the null bias.
atommota 0:a070fa765ed2 104 void calibrateAccelerometer(void);
atommota 0:a070fa765ed2 105 //Take a set of samples and average them.
atommota 0:a070fa765ed2 106 void sampleAccelerometer(void);
atommota 0:a070fa765ed2 107 //Set up the ITG3200 appropriately.
atommota 0:a070fa765ed2 108 void initializeGyroscope(void);
atommota 0:a070fa765ed2 109 //Calculate the null bias.
atommota 0:a070fa765ed2 110 void calibrateGyroscope(void);
atommota 0:a070fa765ed2 111 //Take a set of samples and average them.
atommota 0:a070fa765ed2 112 void sampleGyroscope(void);
atommota 0:a070fa765ed2 113 //Update the filter and calculate the Euler angles.
atommota 0:a070fa765ed2 114 void filter(void);
guqinchen 1:c8232c909f29 115 void processMove(void);
guqinchen 1:c8232c909f29 116 void processClick(void);
atommota 0:a070fa765ed2 117
atommota 0:a070fa765ed2 118 void initializeAccelerometer(void) {
atommota 0:a070fa765ed2 119
atommota 0:a070fa765ed2 120 //Go into standby mode to configure the device.
guqinchen 1:c8232c909f29 121 accelerometer.setPowerControl(0x00);
atommota 0:a070fa765ed2 122 //Full resolution, +/-16g, 4mg/LSB.
guqinchen 1:c8232c909f29 123 accelerometer.setDataFormatControl(0x0B);
atommota 0:a070fa765ed2 124 //200Hz data rate.
guqinchen 1:c8232c909f29 125 accelerometer.setDataRate(ADXL345_1600HZ);
atommota 0:a070fa765ed2 126 //Measurement mode.
guqinchen 1:c8232c909f29 127 accelerometer.setPowerControl(0x08);
atommota 0:a070fa765ed2 128 //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
atommota 0:a070fa765ed2 129 wait_ms(22);
atommota 0:a070fa765ed2 130
atommota 0:a070fa765ed2 131 }
atommota 0:a070fa765ed2 132
guqinchen 1:c8232c909f29 133 void initializeGyroscope(void) {
guqinchen 1:c8232c909f29 134
guqinchen 1:c8232c909f29 135 //Low pass filter bandwidth of 42Hz.
guqinchen 1:c8232c909f29 136 gyroscope.setLpBandwidth(LPFBW_42HZ);
guqinchen 1:c8232c909f29 137 gyroscope.setSampleRateDivider(0);
guqinchen 1:c8232c909f29 138 pc.printf("%d\n", gyroscope.getSampleRateDivider());
guqinchen 1:c8232c909f29 139 pc.printf("%d\n", gyroscope.getInternalSampleRate());
guqinchen 1:c8232c909f29 140 wait_ms(22);
guqinchen 1:c8232c909f29 141 }
guqinchen 1:c8232c909f29 142
guqinchen 1:c8232c909f29 143
guqinchen 1:c8232c909f29 144 void calibrateAccelerometer(void) {
guqinchen 1:c8232c909f29 145
guqinchen 1:c8232c909f29 146 pc.printf("Calibrating Accelerometer\n");
guqinchen 1:c8232c909f29 147 a_xAccumulator = 0;
guqinchen 1:c8232c909f29 148 a_yAccumulator = 0;
guqinchen 1:c8232c909f29 149 a_zAccumulator = 0;
guqinchen 1:c8232c909f29 150
guqinchen 1:c8232c909f29 151 //Take a number of readings and average them
guqinchen 1:c8232c909f29 152 //to calculate the zero g offset.
guqinchen 1:c8232c909f29 153 for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
guqinchen 1:c8232c909f29 154
guqinchen 1:c8232c909f29 155 accelerometer.getOutput(readings);
guqinchen 1:c8232c909f29 156 a_xAccumulator += (int16_t) readings[0];
guqinchen 1:c8232c909f29 157 a_yAccumulator += (int16_t) readings[1];
guqinchen 1:c8232c909f29 158 a_zAccumulator += (int16_t) readings[2];
guqinchen 1:c8232c909f29 159
guqinchen 1:c8232c909f29 160
guqinchen 1:c8232c909f29 161 wait(ACC_RATE);
guqinchen 1:c8232c909f29 162
guqinchen 1:c8232c909f29 163 }
guqinchen 1:c8232c909f29 164
guqinchen 1:c8232c909f29 165 a_xAccumulator /= CALIBRATION_SAMPLES;
guqinchen 1:c8232c909f29 166 a_yAccumulator /= CALIBRATION_SAMPLES;
guqinchen 1:c8232c909f29 167 a_zAccumulator /= CALIBRATION_SAMPLES;
guqinchen 1:c8232c909f29 168
guqinchen 1:c8232c909f29 169 //At 4mg/LSB, 250 LSBs is 1g.
guqinchen 1:c8232c909f29 170 a_xBias = a_xAccumulator;
guqinchen 1:c8232c909f29 171 a_yBias = a_yAccumulator;
guqinchen 1:c8232c909f29 172 a_zBias = (a_zAccumulator - 981);
guqinchen 1:c8232c909f29 173
guqinchen 1:c8232c909f29 174 a_xAccumulator = 0;
guqinchen 1:c8232c909f29 175 a_yAccumulator = 0;
guqinchen 1:c8232c909f29 176 a_zAccumulator = 0;
guqinchen 1:c8232c909f29 177 pc.printf("Calibration Complete\n");
guqinchen 1:c8232c909f29 178 }
guqinchen 1:c8232c909f29 179
guqinchen 1:c8232c909f29 180
guqinchen 1:c8232c909f29 181
guqinchen 1:c8232c909f29 182 void calibrateGyroscope(void) {
guqinchen 1:c8232c909f29 183
guqinchen 1:c8232c909f29 184 pc.printf("Calibrating Gyro\n");
guqinchen 1:c8232c909f29 185 w_xAccumulator = 0;
guqinchen 1:c8232c909f29 186 w_yAccumulator = 0;
guqinchen 1:c8232c909f29 187 w_zAccumulator = 0;
guqinchen 1:c8232c909f29 188
guqinchen 1:c8232c909f29 189 //Take a number of readings and average them
guqinchen 1:c8232c909f29 190 //to calculate the gyroscope bias offset.
guqinchen 1:c8232c909f29 191 for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
guqinchen 1:c8232c909f29 192
guqinchen 1:c8232c909f29 193 w_xAccumulator += gyroscope.getGyroX();
guqinchen 1:c8232c909f29 194 w_yAccumulator += gyroscope.getGyroY();
guqinchen 1:c8232c909f29 195 w_zAccumulator += gyroscope.getGyroZ();
guqinchen 1:c8232c909f29 196 wait(GYRO_RATE);
guqinchen 1:c8232c909f29 197 }
guqinchen 1:c8232c909f29 198
guqinchen 1:c8232c909f29 199 //Average the samples.
guqinchen 1:c8232c909f29 200 w_xAccumulator /= CALIBRATION_SAMPLES;
guqinchen 1:c8232c909f29 201 w_yAccumulator /= CALIBRATION_SAMPLES;
guqinchen 1:c8232c909f29 202 w_zAccumulator /= CALIBRATION_SAMPLES;
guqinchen 1:c8232c909f29 203
guqinchen 1:c8232c909f29 204 w_xBias = w_xAccumulator;
guqinchen 1:c8232c909f29 205 w_yBias = w_yAccumulator;
guqinchen 1:c8232c909f29 206 w_zBias = w_zAccumulator;
guqinchen 1:c8232c909f29 207
guqinchen 1:c8232c909f29 208 w_xAccumulator = 0;
guqinchen 1:c8232c909f29 209 w_yAccumulator = 0;
guqinchen 1:c8232c909f29 210 w_zAccumulator = 0;
guqinchen 1:c8232c909f29 211
guqinchen 1:c8232c909f29 212 pc.printf("Calibration Complete\n");
guqinchen 1:c8232c909f29 213 }
guqinchen 1:c8232c909f29 214
guqinchen 1:c8232c909f29 215
atommota 0:a070fa765ed2 216 void sampleAccelerometer(void) {
atommota 0:a070fa765ed2 217
atommota 0:a070fa765ed2 218 //Have we taken enough samples?
atommota 0:a070fa765ed2 219 if (accelerometerSamples == SAMPLES) {
atommota 0:a070fa765ed2 220
atommota 0:a070fa765ed2 221 //Average the samples, remove the bias, and calculate the acceleration
atommota 0:a070fa765ed2 222 //in m/s/s.
atommota 0:a070fa765ed2 223 a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
atommota 0:a070fa765ed2 224 a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
atommota 0:a070fa765ed2 225 a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
atommota 0:a070fa765ed2 226
atommota 0:a070fa765ed2 227 a_xAccumulator = 0;
atommota 0:a070fa765ed2 228 a_yAccumulator = 0;
atommota 0:a070fa765ed2 229 a_zAccumulator = 0;
atommota 0:a070fa765ed2 230 accelerometerSamples = 0;
atommota 0:a070fa765ed2 231
atommota 0:a070fa765ed2 232 } else {
atommota 0:a070fa765ed2 233 //Take another sample.
guqinchen 1:c8232c909f29 234 accelerometer.getOutput(readings);
guqinchen 1:c8232c909f29 235 a_xAccumulator += (int16_t) readings[0];
guqinchen 1:c8232c909f29 236 a_yAccumulator += (int16_t) readings[1];
guqinchen 1:c8232c909f29 237 a_zAccumulator += (int16_t) readings[2];
atommota 0:a070fa765ed2 238
atommota 0:a070fa765ed2 239 accelerometerSamples++;
guqinchen 1:c8232c909f29 240 //pc.printf("Sample Accl %d", accelerometerSamples);
atommota 0:a070fa765ed2 241 }
atommota 0:a070fa765ed2 242
atommota 0:a070fa765ed2 243 }
atommota 0:a070fa765ed2 244
atommota 0:a070fa765ed2 245
atommota 0:a070fa765ed2 246 void sampleGyroscope(void) {
atommota 0:a070fa765ed2 247
atommota 0:a070fa765ed2 248 //Have we taken enough samples?
atommota 0:a070fa765ed2 249 if (gyroscopeSamples == SAMPLES) {
atommota 0:a070fa765ed2 250
atommota 0:a070fa765ed2 251 //Average the samples, remove the bias, and calculate the angular
atommota 0:a070fa765ed2 252 //velocity in rad/s.
atommota 0:a070fa765ed2 253 w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
atommota 0:a070fa765ed2 254 w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
atommota 0:a070fa765ed2 255 w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
atommota 0:a070fa765ed2 256
atommota 0:a070fa765ed2 257 w_xAccumulator = 0;
atommota 0:a070fa765ed2 258 w_yAccumulator = 0;
atommota 0:a070fa765ed2 259 w_zAccumulator = 0;
atommota 0:a070fa765ed2 260 gyroscopeSamples = 0;
atommota 0:a070fa765ed2 261
atommota 0:a070fa765ed2 262 } else {
atommota 0:a070fa765ed2 263 //Take another sample.
atommota 0:a070fa765ed2 264 w_xAccumulator += gyroscope.getGyroX();
atommota 0:a070fa765ed2 265 w_yAccumulator += gyroscope.getGyroY();
atommota 0:a070fa765ed2 266 w_zAccumulator += gyroscope.getGyroZ();
atommota 0:a070fa765ed2 267
atommota 0:a070fa765ed2 268 gyroscopeSamples++;
guqinchen 1:c8232c909f29 269 //pc.printf("Sample Gyro %d", gyroscopeSamples);
atommota 0:a070fa765ed2 270 }
atommota 0:a070fa765ed2 271
atommota 0:a070fa765ed2 272 }
atommota 0:a070fa765ed2 273
atommota 0:a070fa765ed2 274 void filter(void) {
atommota 0:a070fa765ed2 275 //Update the filter variables.
atommota 0:a070fa765ed2 276 imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
atommota 0:a070fa765ed2 277 //Calculate the new Euler angles.
atommota 0:a070fa765ed2 278 imuFilter.computeEuler();
atommota 0:a070fa765ed2 279
atommota 0:a070fa765ed2 280 }
atommota 0:a070fa765ed2 281
guqinchen 1:c8232c909f29 282 void processClick()
guqinchen 1:c8232c909f29 283 {
guqinchen 1:c8232c909f29 284 static bool preRightClick = false;
guqinchen 1:c8232c909f29 285
guqinchen 1:c8232c909f29 286 if (leftClick == 0)
guqinchen 1:c8232c909f29 287 {
guqinchen 1:c8232c909f29 288 mouse.press(MOUSE_LEFT);
guqinchen 1:c8232c909f29 289 }
guqinchen 1:c8232c909f29 290 else
guqinchen 1:c8232c909f29 291 {
guqinchen 1:c8232c909f29 292 mouse.release(MOUSE_LEFT);
guqinchen 1:c8232c909f29 293 }
guqinchen 1:c8232c909f29 294
guqinchen 1:c8232c909f29 295 // Right Mouse Click ___ Falling Edge Detection
guqinchen 1:c8232c909f29 296 if (rightClick == 0 && preRightClick == false)
guqinchen 1:c8232c909f29 297 {
guqinchen 1:c8232c909f29 298 preRightClick = true;
guqinchen 1:c8232c909f29 299 }
guqinchen 1:c8232c909f29 300 else if (rightClick == 1 && preRightClick == true)
guqinchen 1:c8232c909f29 301 { preRightClick = false;
guqinchen 1:c8232c909f29 302 mouse.click(MOUSE_RIGHT);
guqinchen 1:c8232c909f29 303 }
guqinchen 1:c8232c909f29 304 }
guqinchen 1:c8232c909f29 305
guqinchen 1:c8232c909f29 306
guqinchen 1:c8232c909f29 307 void processMove(void)
guqinchen 1:c8232c909f29 308 {
guqinchen 1:c8232c909f29 309 int16_t move_x, move_y;
guqinchen 1:c8232c909f29 310
guqinchen 1:c8232c909f29 311
guqinchen 1:c8232c909f29 312 move_x = (int16_t)(-MOVERATE*toDegrees(imuFilter.getRoll()));
guqinchen 1:c8232c909f29 313 move_y = (int16_t)(-MOVERATE*toDegrees(imuFilter.getPitch()));
guqinchen 1:c8232c909f29 314
guqinchen 1:c8232c909f29 315 if (move_x <= MOVETHRESHOLD && move_x >= -MOVETHRESHOLD)
guqinchen 1:c8232c909f29 316 move_x = 0;
guqinchen 1:c8232c909f29 317 else if (move_x > MOVETHRESHOLD){
guqinchen 1:c8232c909f29 318 if (move_x > MAXMOVE+MOVETHRESHOLD) move_x = MAXMOVE;
guqinchen 1:c8232c909f29 319 else move_x -=MOVETHRESHOLD;
guqinchen 1:c8232c909f29 320 }
guqinchen 1:c8232c909f29 321
guqinchen 1:c8232c909f29 322 else{
guqinchen 1:c8232c909f29 323 if (move_x < -MAXMOVE-MOVETHRESHOLD) move_x = -MAXMOVE;
guqinchen 1:c8232c909f29 324 else move_x+=MOVETHRESHOLD;
guqinchen 1:c8232c909f29 325 }
guqinchen 1:c8232c909f29 326
guqinchen 1:c8232c909f29 327
guqinchen 1:c8232c909f29 328 if (move_y <= MOVETHRESHOLD && move_y >= -MOVETHRESHOLD)
guqinchen 1:c8232c909f29 329 move_y = 0;
guqinchen 1:c8232c909f29 330 else if (move_y > MOVETHRESHOLD){
guqinchen 1:c8232c909f29 331 if (move_y > MAXMOVE+MOVETHRESHOLD) move_y = MAXMOVE;
guqinchen 1:c8232c909f29 332 else move_y -=MOVETHRESHOLD;
guqinchen 1:c8232c909f29 333 }
guqinchen 1:c8232c909f29 334
guqinchen 1:c8232c909f29 335 else{
guqinchen 1:c8232c909f29 336 if (move_y < -MAXMOVE-MOVETHRESHOLD) move_y = -MAXMOVE;
guqinchen 1:c8232c909f29 337 else move_y+=MOVETHRESHOLD;
guqinchen 1:c8232c909f29 338 }
guqinchen 1:c8232c909f29 339
guqinchen 1:c8232c909f29 340 pc.printf("move_x = %d, move_ y = %d\n", move_x,move_y);
guqinchen 1:c8232c909f29 341
guqinchen 1:c8232c909f29 342 mouse.move(move_x, move_y);
guqinchen 1:c8232c909f29 343 }
guqinchen 1:c8232c909f29 344
atommota 0:a070fa765ed2 345 int main() {
atommota 0:a070fa765ed2 346
atommota 0:a070fa765ed2 347 //pc.printf("Starting IMU filter test...\n");
atommota 0:a070fa765ed2 348
atommota 0:a070fa765ed2 349 //Initialize inertial sensors.
atommota 0:a070fa765ed2 350 initializeAccelerometer();
atommota 0:a070fa765ed2 351 calibrateAccelerometer();
atommota 0:a070fa765ed2 352 initializeGyroscope();
atommota 0:a070fa765ed2 353 calibrateGyroscope();
atommota 0:a070fa765ed2 354
guqinchen 1:c8232c909f29 355
guqinchen 1:c8232c909f29 356
guqinchen 1:c8232c909f29 357 leftClick.mode(PullUp);
guqinchen 1:c8232c909f29 358 rightClick.mode(PullUp);
atommota 0:a070fa765ed2 359 //pc.printf("Initialized Successfully...\n\r");
atommota 0:a070fa765ed2 360
atommota 0:a070fa765ed2 361 //Set up timers.
atommota 0:a070fa765ed2 362 //Accelerometer data rate is 200Hz, so we'll sample at this speed.
guqinchen 1:c8232c909f29 363 accelerometerTicker.attach(&sampleAccelerometer, GYRO_RATE);
atommota 0:a070fa765ed2 364 //Gyroscope data rate is 200Hz, so we'll sample at this speed.
guqinchen 1:c8232c909f29 365 gyroscopeTicker.attach(&sampleGyroscope, ACC_RATE);
atommota 0:a070fa765ed2 366 //Update the filter variables at the correct rate.
atommota 0:a070fa765ed2 367 filterTicker.attach(&filter, FILTER_RATE);
atommota 0:a070fa765ed2 368
atommota 0:a070fa765ed2 369
atommota 0:a070fa765ed2 370 //pc.printf("Timers Setup...Entering Loop...\n\r");
guqinchen 1:c8232c909f29 371
atommota 0:a070fa765ed2 372
atommota 0:a070fa765ed2 373 while (1) {
atommota 0:a070fa765ed2 374
guqinchen 1:c8232c909f29 375 wait(0.01);
guqinchen 1:c8232c909f29 376 pc.printf("angle_x = %f, angle_ y = %f\n",-toDegrees(imuFilter.getRoll()),-toDegrees(imuFilter.getPitch()));
guqinchen 1:c8232c909f29 377 processClick();
guqinchen 1:c8232c909f29 378 processMove();
atommota 0:a070fa765ed2 379 }
atommota 0:a070fa765ed2 380
atommota 0:a070fa765ed2 381 }