Maxwell Poster / Mbed 2 deprecated CS335_Final_Project_i2c_MPU6050_Gyro_Mouse

Final project for CS335 By Maxwell Poster and Jeffrey Resnik

Dependencies:   mbed MPU6050_template ledControl2 USBDevice

main.cpp@14:e6e3099e3b3b, 2020-12-02 (annotated)

Committer:
mposter
Date:
Wed Dec 02 00:19:20 2020 +0000
Revision:
14:e6e3099e3b3b
Parent:
13:e9fe7586ab0c 
Mini cleanup

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mposter 14:e6e3099e3b3b 1 /*
BaserK 0:9203a021a0be 2 *
BaserK 0:9203a021a0be 3 * @connections:
BaserK 0:9203a021a0be 4 *--------------------------------------------------------------
BaserK 0:9203a021a0be 5 * |LPC1768| |Peripherals|
BaserK 0:9203a021a0be 6 * Pin 9 ---------> SDA of MPU6050
BaserK 0:9203a021a0be 7 * Pin 10 --------> SCL of MPU6050
BaserK 4:33fef1998fc8 8 * GND -----------> GND of MPU6050
BaserK 0:9203a021a0be 9 * VOUT (3.3 V) --> VCC of MPU6050
BaserK 0:9203a021a0be 10 *---------------------------------------------------------------
BaserK 6:12ff524abb6a 11 *--------------------------------------------------------------
BaserK 6:12ff524abb6a 12 * |NUCLEO F411RE| |Peripherals|
BaserK 6:12ff524abb6a 13 * D14 -----------> SDA of MPU6050
BaserK 6:12ff524abb6a 14 * D15 -----------> SCL of MPU6050
BaserK 6:12ff524abb6a 15 * GND -----------> GND of MPU6050
BaserK 6:12ff524abb6a 16 * VOUT (3.3 V) --> VCC of MPU6050
BaserK 6:12ff524abb6a 17 *---------------------------------------------------------------
BaserK 0:9203a021a0be 18 */
BaserK 0:9203a021a0be 19
BaserK 0:9203a021a0be 20 #include "mbed.h"
BaserK 0:9203a021a0be 21 #include "MPU6050.h"
BaserK 0:9203a021a0be 22 #include "ledControl.h"
mposter 13:e9fe7586ab0c 23 #include "USBMouse.h"
BaserK 0:9203a021a0be 24
mposter 13:e9fe7586ab0c 25 #include <deque> // std::queue
mposter 13:e9fe7586ab0c 26 #include <math.h>
mposter 13:e9fe7586ab0c 27
mposter 13:e9fe7586ab0c 28 #define QUEUE_LENGTH 5
mposter 13:e9fe7586ab0c 29 #define SCREEN_DISTANCE 0.5
mposter 13:e9fe7586ab0c 30 #define RESOLUTION 100
mposter 13:e9fe7586ab0c 31 #define ACCEL_RESOLUTION 500
mposter 13:e9fe7586ab0c 32 #define SCROLLUP_LENGTH 5
mposter 13:e9fe7586ab0c 33 #define SCROLLDOWN_LENGTH 5
remig 8:7b491d7b6d9d 34 //! Variable debug
remig 8:7b491d7b6d9d 35 #define DEBUG
mposter 14:e6e3099e3b3b 36 /* */
BaserK 0:9203a021a0be 37
remig 8:7b491d7b6d9d 38 //! Instance pc de classe Serial
mposter 13:e9fe7586ab0c 39 //Serial pc(USBTX,USBRX); // default baud rate: 9600
remig 8:7b491d7b6d9d 40 //! Instance mpu6050 de classe MPU6050
BaserK 4:33fef1998fc8 41 MPU6050 mpu6050; // class: MPU6050, object: mpu6050
mposter 14:e6e3099e3b3b 42
remig 8:7b491d7b6d9d 43 // Ticker
remig 8:7b491d7b6d9d 44 // A Ticker is used to call a function at a recurring interval.
remig 8:7b491d7b6d9d 45 // You can use as many separate Ticker objects as you require.
BaserK 0:9203a021a0be 46 Ticker toggler1;
BaserK 3:88737ad5c803 47 Ticker filter;
BaserK 0:9203a021a0be 48
mposter 13:e9fe7586ab0c 49 //USBMouse objcet
mposter 13:e9fe7586ab0c 50 USBMouse mouse;
mposter 13:e9fe7586ab0c 51
BaserK 0:9203a021a0be 52 void toggle_led1();
BaserK 0:9203a021a0be 53 void toggle_led2();
BaserK 3:88737ad5c803 54 void compFilter();
BaserK 3:88737ad5c803 55
mposter 14:e6e3099e3b3b 56 //Not used by us, but required to apply the filter from the MPU lib
BaserK 3:88737ad5c803 57 float pitchAngle = 0;
BaserK 3:88737ad5c803 58 float rollAngle = 0;
BaserK 0:9203a021a0be 59
mposter 13:e9fe7586ab0c 60 //My funcs
mposter 13:e9fe7586ab0c 61 void calibrateGyro();
mposter 13:e9fe7586ab0c 62 void calibrateAccel();
mposter 13:e9fe7586ab0c 63 float sampleQueue(deque<float> q,float bias);
mposter 13:e9fe7586ab0c 64 void clearDeques();
mposter 13:e9fe7586ab0c 65 void updateDeques();
mposter 13:e9fe7586ab0c 66 int16_t processGX();
mposter 13:e9fe7586ab0c 67 int16_t processGZ();
mposter 13:e9fe7586ab0c 68 void processGyro();
mposter 13:e9fe7586ab0c 69 int16_t processAX();
mposter 13:e9fe7586ab0c 70 int16_t processAZ();
mposter 13:e9fe7586ab0c 71 void processAccel();
mposter 13:e9fe7586ab0c 72 void processMovement();
mposter 13:e9fe7586ab0c 73 void checkClicks();
mposter 13:e9fe7586ab0c 74 //end my funcs
mposter 13:e9fe7586ab0c 75
mposter 13:e9fe7586ab0c 76 //Create queue to insert gyroReadings into
mposter 13:e9fe7586ab0c 77 deque<float> gyroReadingsx;
mposter 13:e9fe7586ab0c 78 deque<float> gyroReadingsz;
mposter 13:e9fe7586ab0c 79
mposter 13:e9fe7586ab0c 80 //Create queue to insert accelReadings into
mposter 13:e9fe7586ab0c 81 deque<float> accelReadingsx;
mposter 13:e9fe7586ab0c 82 deque<float> accelReadingsz;
mposter 13:e9fe7586ab0c 83
mposter 13:e9fe7586ab0c 84 //Predefined gyro bias. Subtract these from queue averages to get movement weight
mposter 13:e9fe7586ab0c 85 float gyroBiasx = 0;
mposter 13:e9fe7586ab0c 86 float gyroBiasz = 0;
mposter 13:e9fe7586ab0c 87
mposter 13:e9fe7586ab0c 88 //Predefined accel bias. Subtract these from queue averages to get movement weight
mposter 13:e9fe7586ab0c 89 float accelBiasx = 0;
mposter 13:e9fe7586ab0c 90 float accelBiasz = 0;
mposter 13:e9fe7586ab0c 91
mposter 13:e9fe7586ab0c 92 //Clicking pins
mposter 13:e9fe7586ab0c 93 DigitalIn leftClick(p16);
mposter 13:e9fe7586ab0c 94 DigitalIn rightClick(p15);
mposter 13:e9fe7586ab0c 95
mposter 13:e9fe7586ab0c 96 //Scroll pins
mposter 13:e9fe7586ab0c 97 DigitalIn upScroll(p18);
mposter 13:e9fe7586ab0c 98 DigitalIn downScroll(p19);
mposter 13:e9fe7586ab0c 99
mposter 13:e9fe7586ab0c 100 //Pause pin
mposter 13:e9fe7586ab0c 101 DigitalIn pause(p20);
mposter 13:e9fe7586ab0c 102
BaserK 0:9203a021a0be 103 int main()
BaserK 0:9203a021a0be 104 {
mposter 14:e6e3099e3b3b 105
mposter 14:e6e3099e3b3b 106 //Begin initialize*********************************************************
mposter 14:e6e3099e3b3b 107
mposter 14:e6e3099e3b3b 108 //Imported https://os.mbed.com/users/remig/code/i2c_MPU6050//file/369106aa267a/main.cpp/
mposter 13:e9fe7586ab0c 109 ////pc.baud(9600); // baud rate: 9600
BaserK 0:9203a021a0be 110 mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
BaserK 0:9203a021a0be 111 wait(1);
BaserK 0:9203a021a0be 112 mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
mposter 13:e9fe7586ab0c 113 ////////pc.printf("Calibration is completed. \r\n");
BaserK 0:9203a021a0be 114 wait(0.5);
BaserK 0:9203a021a0be 115 mpu6050.init(); // Initialize the sensor
BaserK 0:9203a021a0be 116 wait(1);
mposter 13:e9fe7586ab0c 117 ////////pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
BaserK 0:9203a021a0be 118 wait_ms(500);
mposter 13:e9fe7586ab0c 119 //End imported
mposter 13:e9fe7586ab0c 120
mposter 14:e6e3099e3b3b 121
mposter 14:e6e3099e3b3b 122
mposter 13:e9fe7586ab0c 123 //Perform our own sampling calibration
mposter 13:e9fe7586ab0c 124 for(int i = 0; i < QUEUE_LENGTH; i++){
mposter 13:e9fe7586ab0c 125 filter.attach(&compFilter, 0.005);
mposter 13:e9fe7586ab0c 126 updateDeques();
mposter 13:e9fe7586ab0c 127 }
mposter 14:e6e3099e3b3b 128
mposter 14:e6e3099e3b3b 129 //Calculate biases
mposter 13:e9fe7586ab0c 130 calibrateGyro();
mposter 13:e9fe7586ab0c 131 calibrateAccel();
mposter 13:e9fe7586ab0c 132
mposter 14:e6e3099e3b3b 133 //End intialize***********************************************************
mposter 14:e6e3099e3b3b 134
mposter 13:e9fe7586ab0c 135 ////////pc.printf("Bias x: %.3f | Bias z: %.3f\r\n",gyroBiasx,gyroBiasz);
mposter 13:e9fe7586ab0c 136
mposter 13:e9fe7586ab0c 137 //wait(2.5);
BaserK 0:9203a021a0be 138
BaserK 0:9203a021a0be 139 while(1)
BaserK 0:9203a021a0be 140 {
mposter 13:e9fe7586ab0c 141
mposter 13:e9fe7586ab0c 142
BaserK 4:33fef1998fc8 143
mposter 13:e9fe7586ab0c 144 //////////pc.printf(" _____________________________________________________________ \r\n");
mposter 13:e9fe7586ab0c 145 //////////pc.printf("| Accelerometer(g) | ax=%.3f | ay=%.3f | az=%.3f \r\n",ax,ay,az);
mposter 14:e6e3099e3b3b 146 //printf("| Gyroscope(deg/s) | gx=%.3f | gy=%.3f | gz=%.3f \r\n",gx,gy,gz);
mposter 13:e9fe7586ab0c 147 //////////pc.printf("|_____________________________________________________________ \r\n\r\n");
mposter 13:e9fe7586ab0c 148 //
mposter 13:e9fe7586ab0c 149 //X AXIS GYRO
mposter 13:e9fe7586ab0c 150 //////pc.printf("x axis (gyro): ");
mposter 13:e9fe7586ab0c 151 //for(int i = 0; i < QUEUE_LENGTH; i++){
mposter 13:e9fe7586ab0c 152 //////////pc.printf("%.3f ",gyroReadingsx[i]);
mposter 13:e9fe7586ab0c 153 //}
mposter 13:e9fe7586ab0c 154 //////pc.printf("\r\n");
BaserK 0:9203a021a0be 155
mposter 13:e9fe7586ab0c 156 //Z AXIS GYRO
mposter 13:e9fe7586ab0c 157 //////pc.printf("z axis (gyro): ");
mposter 13:e9fe7586ab0c 158 //for(int i = 0; i < QUEUE_LENGTH; i++){
mposter 13:e9fe7586ab0c 159 //////////pc.printf("%.3f ",gyroReadingsz[i]);
mposter 13:e9fe7586ab0c 160 //}
mposter 13:e9fe7586ab0c 161 //////pc.printf("\r\n");
mposter 13:e9fe7586ab0c 162 //wait(2.5);
mposter 13:e9fe7586ab0c 163
mposter 13:e9fe7586ab0c 164
mposter 13:e9fe7586ab0c 165 filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
BaserK 0:9203a021a0be 166
mposter 13:e9fe7586ab0c 167 updateDeques();
mposter 13:e9fe7586ab0c 168 processMovement();
mposter 13:e9fe7586ab0c 169 checkClicks();
mposter 13:e9fe7586ab0c 170
mposter 14:e6e3099e3b3b 171 //wait(2.5);
BaserK 0:9203a021a0be 172 }
BaserK 0:9203a021a0be 173 }
BaserK 0:9203a021a0be 174
mposter 13:e9fe7586ab0c 175 //Function to check and process button clicks for left and right mouse buttons
mposter 13:e9fe7586ab0c 176 void checkClicks(){
mposter 13:e9fe7586ab0c 177
mposter 13:e9fe7586ab0c 178 //Imported
mposter 14:e6e3099e3b3b 179 //https://os.mbed.com/users/guqinchen/code/P4_IMU//file/044740372a78/main.cpp/
mposter 13:e9fe7586ab0c 180 static bool preRightClick = false;
mposter 13:e9fe7586ab0c 181
mposter 13:e9fe7586ab0c 182 if (leftClick)
mposter 13:e9fe7586ab0c 183 {
mposter 13:e9fe7586ab0c 184 mouse.press(MOUSE_LEFT);
mposter 13:e9fe7586ab0c 185 }
mposter 13:e9fe7586ab0c 186 else
mposter 13:e9fe7586ab0c 187 {
mposter 13:e9fe7586ab0c 188 mouse.release(MOUSE_LEFT);
mposter 13:e9fe7586ab0c 189 }
mposter 13:e9fe7586ab0c 190
mposter 13:e9fe7586ab0c 191 // Right Mouse Click ___ Falling Edge Detection
mposter 13:e9fe7586ab0c 192 if (rightClick && !preRightClick)
mposter 13:e9fe7586ab0c 193 {
mposter 13:e9fe7586ab0c 194 preRightClick = true;
mposter 13:e9fe7586ab0c 195 }
mposter 13:e9fe7586ab0c 196 else if (!rightClick && preRightClick)
mposter 13:e9fe7586ab0c 197 { preRightClick = false;
mposter 13:e9fe7586ab0c 198 mouse.click(MOUSE_RIGHT);
mposter 13:e9fe7586ab0c 199 }
mposter 13:e9fe7586ab0c 200 //End imported
mposter 13:e9fe7586ab0c 201
mposter 13:e9fe7586ab0c 202 //Listen for scroll up button
mposter 13:e9fe7586ab0c 203 if(upScroll){
mposter 13:e9fe7586ab0c 204 for(int i = 0; i < SCROLLUP_LENGTH; i++){
mposter 13:e9fe7586ab0c 205 mouse.scroll(-1);
mposter 13:e9fe7586ab0c 206 }
mposter 13:e9fe7586ab0c 207 mouse.release(MOUSE_MIDDLE);
mposter 13:e9fe7586ab0c 208 }
mposter 13:e9fe7586ab0c 209
mposter 13:e9fe7586ab0c 210 //Listen for scroll down button
mposter 13:e9fe7586ab0c 211 if(downScroll){
mposter 13:e9fe7586ab0c 212 for(int i = 0; i < SCROLLDOWN_LENGTH; i++){
mposter 13:e9fe7586ab0c 213 mouse.scroll(1);
mposter 13:e9fe7586ab0c 214 }
mposter 13:e9fe7586ab0c 215 mouse.release(MOUSE_MIDDLE);
mposter 13:e9fe7586ab0c 216 }
mposter 13:e9fe7586ab0c 217
mposter 13:e9fe7586ab0c 218 //Loop while pause button is pressed
mposter 13:e9fe7586ab0c 219 while(pause);
mposter 13:e9fe7586ab0c 220 }
mposter 13:e9fe7586ab0c 221
mposter 13:e9fe7586ab0c 222 //Only care about x and z axis
mposter 13:e9fe7586ab0c 223 // -z -> Right side of screen; +z -> Left side of screen
mposter 13:e9fe7586ab0c 224 // -x - > Bottom of screen; +x -> Top of screen
mposter 13:e9fe7586ab0c 225
mposter 13:e9fe7586ab0c 226 int16_t processGX(){
mposter 13:e9fe7586ab0c 227
mposter 13:e9fe7586ab0c 228 //Obtain sample from deque
mposter 13:e9fe7586ab0c 229 float sample = sampleQueue(gyroReadingsx,gyroBiasx);
mposter 13:e9fe7586ab0c 230
mposter 13:e9fe7586ab0c 231 //Convert to radians
mposter 13:e9fe7586ab0c 232 float sampleRadians = sample*PI/180.0;
mposter 13:e9fe7586ab0c 233
mposter 14:e6e3099e3b3b 234 //Compute distance to move
mposter 13:e9fe7586ab0c 235 float deltaX = SCREEN_DISTANCE*RESOLUTION*tan(sampleRadians);
mposter 13:e9fe7586ab0c 236
mposter 14:e6e3099e3b3b 237 //Cast to int16_t
mposter 13:e9fe7586ab0c 238 int16_t sending = 0 + deltaX;
mposter 13:e9fe7586ab0c 239
mposter 13:e9fe7586ab0c 240 printf("x axis MOVING: %.3f, sending: %i\r\n",deltaX,sending);
mposter 13:e9fe7586ab0c 241
mposter 13:e9fe7586ab0c 242 //Do tangent calculation to obtain opposite length
mposter 13:e9fe7586ab0c 243 //x = L*tan(theta)
mposter 13:e9fe7586ab0c 244 //mouse.move(0,sending*-1);
mposter 13:e9fe7586ab0c 245
mposter 13:e9fe7586ab0c 246 return -1*sending;
mposter 13:e9fe7586ab0c 247 }
mposter 13:e9fe7586ab0c 248
mposter 13:e9fe7586ab0c 249 int16_t processGZ(){
mposter 13:e9fe7586ab0c 250
mposter 13:e9fe7586ab0c 251 //Obtain sample from deque
mposter 13:e9fe7586ab0c 252 float sample = sampleQueue(gyroReadingsz,gyroBiasz);
mposter 13:e9fe7586ab0c 253
mposter 13:e9fe7586ab0c 254 //////////////pc.printf("sample y: %.3f \r\n", sample);
mposter 13:e9fe7586ab0c 255
mposter 13:e9fe7586ab0c 256 //Convert to radians
mposter 13:e9fe7586ab0c 257 float sampleRadians = sample*PI/180.0;
mposter 13:e9fe7586ab0c 258
mposter 13:e9fe7586ab0c 259 //////pc.printf("sample (Radians) y: %.3f \r\n", sampleRadians);
mposter 13:e9fe7586ab0c 260
mposter 14:e6e3099e3b3b 261 //Compute distance to move
mposter 13:e9fe7586ab0c 262 float deltaZ = SCREEN_DISTANCE*RESOLUTION*tan(sampleRadians);
mposter 13:e9fe7586ab0c 263
mposter 13:e9fe7586ab0c 264 //Apply multipliers to enable mouse acceleration
mposter 13:e9fe7586ab0c 265 if(deltaZ > 0){
mposter 13:e9fe7586ab0c 266 deltaZ*=1.75;
mposter 13:e9fe7586ab0c 267 }else{
mposter 14:e6e3099e3b3b 268 //If the amount to move z was negative, it tended to be stronger than the amount to move the z in the positive
mposter 14:e6e3099e3b3b 269 // so we weighted negative z deltas less than positive ones
mposter 13:e9fe7586ab0c 270 deltaZ*=1.5;
mposter 13:e9fe7586ab0c 271 }
mposter 13:e9fe7586ab0c 272
mposter 13:e9fe7586ab0c 273 //////pc.printf("without cast: %.3f",SCREEN_DISTANCE*RESOLUTION*tan(sampleRadians));
mposter 13:e9fe7586ab0c 274
mposter 13:e9fe7586ab0c 275 int16_t sending = 0 + deltaZ;
mposter 13:e9fe7586ab0c 276
mposter 13:e9fe7586ab0c 277 printf("z axis MOVING: %.3f, sending: %i\r\n",deltaZ,sending);
mposter 13:e9fe7586ab0c 278
mposter 13:e9fe7586ab0c 279 //Do tangent calculation to obtain opposite length
mposter 13:e9fe7586ab0c 280 //x = L*tan(theta)
mposter 13:e9fe7586ab0c 281 //mouse.move(sending*-1,0);
mposter 13:e9fe7586ab0c 282 return -1*sending;
mposter 13:e9fe7586ab0c 283 }
mposter 13:e9fe7586ab0c 284
mposter 13:e9fe7586ab0c 285 int16_t processAX(){
mposter 13:e9fe7586ab0c 286
mposter 13:e9fe7586ab0c 287 //Obtain sample from deque
mposter 13:e9fe7586ab0c 288 float sample = sampleQueue(accelReadingsx,accelBiasx);
mposter 13:e9fe7586ab0c 289
mposter 13:e9fe7586ab0c 290 int16_t sending = 0 + sample*ACCEL_RESOLUTION;
mposter 13:e9fe7586ab0c 291
mposter 13:e9fe7586ab0c 292 printf("ax axis MOVING: %.3f, sending: %i\r\n",sample,sending);
mposter 13:e9fe7586ab0c 293
mposter 13:e9fe7586ab0c 294 //Do tangent calculation to obtain opposite length
mposter 13:e9fe7586ab0c 295 ////x = L*tan(theta)
mposter 13:e9fe7586ab0c 296 //mouse.move(0,sending*-1);
mposter 13:e9fe7586ab0c 297 return -1*sending;
mposter 13:e9fe7586ab0c 298 }
mposter 13:e9fe7586ab0c 299
mposter 13:e9fe7586ab0c 300 int16_t processAZ(){
mposter 13:e9fe7586ab0c 301
mposter 13:e9fe7586ab0c 302 //Obtain sample from deque
mposter 13:e9fe7586ab0c 303 float sample = sampleQueue(accelReadingsz,accelBiasz);
mposter 13:e9fe7586ab0c 304
mposter 13:e9fe7586ab0c 305 //////////////pc.printf("sample y: %.3f \r\n", sample);
mposter 13:e9fe7586ab0c 306
mposter 13:e9fe7586ab0c 307 //////pc.printf("without cast: %.3f",SCREEN_DISTANCE*RESOLUTION*tan(sampleRadians));
mposter 13:e9fe7586ab0c 308
mposter 13:e9fe7586ab0c 309 int16_t sending = 0 + sample*ACCEL_RESOLUTION;
mposter 13:e9fe7586ab0c 310
mposter 13:e9fe7586ab0c 311 printf("az axis MOVING: %.3f, sending: %i\r\n",sample,sending);
mposter 13:e9fe7586ab0c 312
mposter 13:e9fe7586ab0c 313 //Do tangent calculation to obtain opposite length
mposter 13:e9fe7586ab0c 314 //x = L*tan(theta)
mposter 13:e9fe7586ab0c 315 //mouse.move(sending*-1,0);
mposter 13:e9fe7586ab0c 316 return -1*sending;
mposter 13:e9fe7586ab0c 317 }
mposter 13:e9fe7586ab0c 318
mposter 13:e9fe7586ab0c 319 void processGyro(){
mposter 13:e9fe7586ab0c 320 int16_t y = processGX();
mposter 13:e9fe7586ab0c 321 int16_t x = processGZ();
mposter 13:e9fe7586ab0c 322 mouse.move(x,y);
mposter 13:e9fe7586ab0c 323 }
mposter 13:e9fe7586ab0c 324
mposter 13:e9fe7586ab0c 325 void processAccel(){
mposter 13:e9fe7586ab0c 326 int16_t y = processAX();
mposter 13:e9fe7586ab0c 327 int16_t x = processAZ();
mposter 13:e9fe7586ab0c 328 mouse.move(x,y);
mposter 13:e9fe7586ab0c 329 }
mposter 13:e9fe7586ab0c 330
mposter 13:e9fe7586ab0c 331 void processMovement(){
mposter 13:e9fe7586ab0c 332 processGyro();
mposter 13:e9fe7586ab0c 333 //processAccel();
mposter 13:e9fe7586ab0c 334 }
mposter 13:e9fe7586ab0c 335
mposter 13:e9fe7586ab0c 336 void updateDeques(){
mposter 13:e9fe7586ab0c 337
mposter 13:e9fe7586ab0c 338 //Update gyro deques
mposter 13:e9fe7586ab0c 339 gyroReadingsx.pop_back();
mposter 13:e9fe7586ab0c 340 gyroReadingsz.pop_back();
mposter 13:e9fe7586ab0c 341 gyroReadingsx.push_front(gx);
mposter 13:e9fe7586ab0c 342 gyroReadingsz.push_front(gz);
mposter 13:e9fe7586ab0c 343
mposter 13:e9fe7586ab0c 344 //Update accel deques
mposter 13:e9fe7586ab0c 345 accelReadingsx.pop_back();
mposter 13:e9fe7586ab0c 346 accelReadingsz.pop_back();
mposter 13:e9fe7586ab0c 347 accelReadingsx.push_front(ax);
mposter 13:e9fe7586ab0c 348 accelReadingsz.push_front(az);
mposter 13:e9fe7586ab0c 349 }
mposter 13:e9fe7586ab0c 350
mposter 13:e9fe7586ab0c 351 void clearDeques(){
mposter 13:e9fe7586ab0c 352
mposter 13:e9fe7586ab0c 353 //Check for bad queue size
mposter 13:e9fe7586ab0c 354 if(gyroReadingsx.size() < QUEUE_LENGTH || gyroReadingsz.size() < QUEUE_LENGTH || accelReadingsx.size() < QUEUE_LENGTH || accelReadingsz.size() < QUEUE_LENGTH){
mposter 13:e9fe7586ab0c 355 while(1) printf("Bad queue size. Hanging...\n");
mposter 13:e9fe7586ab0c 356 }
mposter 13:e9fe7586ab0c 357
mposter 13:e9fe7586ab0c 358 //Clear gyro deques
mposter 13:e9fe7586ab0c 359 for(int i = 0; i < QUEUE_LENGTH; i++){
mposter 13:e9fe7586ab0c 360 gyroReadingsx.pop_back();
mposter 13:e9fe7586ab0c 361 gyroReadingsz.pop_back();
mposter 13:e9fe7586ab0c 362 }
mposter 13:e9fe7586ab0c 363
mposter 13:e9fe7586ab0c 364 //Clear accel deques
mposter 13:e9fe7586ab0c 365 for(int i = 0; i < QUEUE_LENGTH; i++){
mposter 13:e9fe7586ab0c 366 accelReadingsx.pop_back();
mposter 13:e9fe7586ab0c 367 accelReadingsz.pop_back();
mposter 13:e9fe7586ab0c 368 }
mposter 13:e9fe7586ab0c 369 }
mposter 13:e9fe7586ab0c 370
mposter 13:e9fe7586ab0c 371 void calibrateGyro(){
mposter 13:e9fe7586ab0c 372 //Only care about x and z axis
mposter 13:e9fe7586ab0c 373 // -z -> Right side of screen; +z -> Left side of screen
mposter 13:e9fe7586ab0c 374 // -x - > Bottom of screen; +x -> Top of screen
mposter 13:e9fe7586ab0c 375
mposter 13:e9fe7586ab0c 376 float xSum = 0;
mposter 13:e9fe7586ab0c 377 float zSum = 0;
mposter 13:e9fe7586ab0c 378
mposter 13:e9fe7586ab0c 379 //Iterate for QUEUE_LENGTH samples from gyroscope
mposter 13:e9fe7586ab0c 380 for(int i = 0; i < QUEUE_LENGTH; i++){
mposter 13:e9fe7586ab0c 381
mposter 13:e9fe7586ab0c 382 float xReading = gx;
mposter 13:e9fe7586ab0c 383 float zReading = gz;
mposter 13:e9fe7586ab0c 384
mposter 13:e9fe7586ab0c 385 printf("CALIB GX,GZ: %.3f %.3f \r\n", gx,gz);
mposter 13:e9fe7586ab0c 386
mposter 13:e9fe7586ab0c 387 //Apply filter to incoming readings
mposter 13:e9fe7586ab0c 388 filter.attach(&compFilter, 0.005);
mposter 13:e9fe7586ab0c 389
mposter 13:e9fe7586ab0c 390 //push gyro x and z readings onto deque
mposter 13:e9fe7586ab0c 391 gyroReadingsx.push_front(xReading);
mposter 13:e9fe7586ab0c 392 gyroReadingsz.push_front(zReading);
mposter 13:e9fe7586ab0c 393
mposter 13:e9fe7586ab0c 394 //add samples to sum totals
mposter 13:e9fe7586ab0c 395 xSum += xReading;
mposter 13:e9fe7586ab0c 396 zSum += zReading;
mposter 13:e9fe7586ab0c 397
mposter 13:e9fe7586ab0c 398 }
mposter 13:e9fe7586ab0c 399
mposter 13:e9fe7586ab0c 400 //Set sampled bias for each gyro axis
mposter 13:e9fe7586ab0c 401 gyroBiasx = xSum/QUEUE_LENGTH;
mposter 13:e9fe7586ab0c 402 gyroBiasz = zSum/QUEUE_LENGTH;
mposter 13:e9fe7586ab0c 403
mposter 13:e9fe7586ab0c 404 }
mposter 13:e9fe7586ab0c 405
mposter 13:e9fe7586ab0c 406 void calibrateAccel(){
mposter 13:e9fe7586ab0c 407 //Only care about x and z axis
mposter 13:e9fe7586ab0c 408 // -z -> Right side of screen; +z -> Left side of screen
mposter 13:e9fe7586ab0c 409 // -x - > Bottom of screen; +x -> Top of screen
mposter 13:e9fe7586ab0c 410
mposter 13:e9fe7586ab0c 411 float xSum = 0;
mposter 13:e9fe7586ab0c 412 float zSum = 0;
mposter 13:e9fe7586ab0c 413
mposter 13:e9fe7586ab0c 414 //Iterate for QUEUE_LENGTH samples from gyroscope
mposter 13:e9fe7586ab0c 415 for(int i = 0; i < QUEUE_LENGTH; i++){
mposter 13:e9fe7586ab0c 416
mposter 13:e9fe7586ab0c 417 float xReading = ax;
mposter 13:e9fe7586ab0c 418 float zReading = az;
mposter 13:e9fe7586ab0c 419
mposter 13:e9fe7586ab0c 420 printf("CALIB AX,AZ: %.3f %.3f \r\n", ax,az);
mposter 13:e9fe7586ab0c 421
mposter 13:e9fe7586ab0c 422 //Apply filter to incoming readings
mposter 13:e9fe7586ab0c 423 filter.attach(&compFilter, 0.005);
mposter 13:e9fe7586ab0c 424
mposter 13:e9fe7586ab0c 425 //push gyro x and z readings onto deque
mposter 13:e9fe7586ab0c 426 accelReadingsx.push_front(xReading);
mposter 13:e9fe7586ab0c 427 accelReadingsz.push_front(zReading);
mposter 13:e9fe7586ab0c 428
mposter 13:e9fe7586ab0c 429 //add samples to sum totals
mposter 13:e9fe7586ab0c 430 xSum += xReading;
mposter 13:e9fe7586ab0c 431 zSum += zReading;
mposter 13:e9fe7586ab0c 432
mposter 13:e9fe7586ab0c 433 }
mposter 13:e9fe7586ab0c 434
mposter 13:e9fe7586ab0c 435 //Set sampled bias for each gyro axis
mposter 13:e9fe7586ab0c 436 accelBiasx = xSum/QUEUE_LENGTH;
mposter 13:e9fe7586ab0c 437 accelBiasz = zSum/QUEUE_LENGTH;
mposter 13:e9fe7586ab0c 438
mposter 13:e9fe7586ab0c 439 }
mposter 13:e9fe7586ab0c 440
mposter 13:e9fe7586ab0c 441 //Calculate average of samples in a deque
mposter 13:e9fe7586ab0c 442 float calculateAverage(deque<float> q){
mposter 13:e9fe7586ab0c 443 float sum = 0;
mposter 13:e9fe7586ab0c 444 for(int i = 0; i < q.size(); i++){
mposter 13:e9fe7586ab0c 445 sum+=q[i];
mposter 13:e9fe7586ab0c 446 }
mposter 13:e9fe7586ab0c 447 return sum/q.size();
mposter 13:e9fe7586ab0c 448 }
mposter 13:e9fe7586ab0c 449
mposter 13:e9fe7586ab0c 450 //Subtract the bias from the average of samples in a deque to offset idle values
mposter 13:e9fe7586ab0c 451 float sampleQueue(deque<float> q,float bias){
mposter 13:e9fe7586ab0c 452
mposter 13:e9fe7586ab0c 453 //Alternatively, may want to calculate the average and subtract next incoming reading by avg
mposter 13:e9fe7586ab0c 454 return calculateAverage(q) - bias;
mposter 13:e9fe7586ab0c 455 }
mposter 13:e9fe7586ab0c 456
mposter 13:e9fe7586ab0c 457 //Imported
BaserK 0:9203a021a0be 458 void toggle_led1() {ledToggle(1);}
BaserK 0:9203a021a0be 459 void toggle_led2() {ledToggle(2);}
BaserK 3:88737ad5c803 460
BaserK 3:88737ad5c803 461 /* This function is created to avoid address error that caused from Ticker.attach func */
BaserK 4:33fef1998fc8 462 void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}
mposter 13:e9fe7586ab0c 463 //End imported