Maxwell Poster / Mbed 2 deprecated i2c_MPU6050_335_template

template for jeff

Dependencies:   mbed MPU6050_template ledControl2 USBDevice

main.cpp@13:e9fe7586ab0c, 2020-11-30 (annotated)

Committer:
mposter
Date:
Mon Nov 30 16:39:57 2020 +0000
Revision:
13:e9fe7586ab0c
Parent:
10:72bbd203b210 
final

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BaserK 4:33fef1998fc8 1 /* Getting Pitch and Roll Angles from MPU6050
BaserK 0:9203a021a0be 2 *
BaserK 0:9203a021a0be 3 * @author: Baser Kandehir
BaserK 4:33fef1998fc8 4 * @date: July 16, 2015
BaserK 5:a6a235d5442d 5 * @license: MIT license
BaserK 5:a6a235d5442d 6 *
BaserK 5:a6a235d5442d 7 * Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr
BaserK 5:a6a235d5442d 8 *
BaserK 5:a6a235d5442d 9 * Permission is hereby granted, free of charge, to any person obtaining a copy
BaserK 5:a6a235d5442d 10 * of this software and associated documentation files (the "Software"), to deal
BaserK 5:a6a235d5442d 11 * in the Software without restriction, including without limitation the rights
BaserK 5:a6a235d5442d 12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
BaserK 5:a6a235d5442d 13 * copies of the Software, and to permit persons to whom the Software is
BaserK 5:a6a235d5442d 14 * furnished to do so, subject to the following conditions:
BaserK 5:a6a235d5442d 15 *
BaserK 5:a6a235d5442d 16 * The above copyright notice and this permission notice shall be included in
BaserK 5:a6a235d5442d 17 * all copies or substantial portions of the Software.
BaserK 5:a6a235d5442d 18 *
BaserK 5:a6a235d5442d 19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
BaserK 5:a6a235d5442d 20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
BaserK 5:a6a235d5442d 21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
BaserK 5:a6a235d5442d 22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
BaserK 5:a6a235d5442d 23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
BaserK 5:a6a235d5442d 24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
BaserK 5:a6a235d5442d 25 * THE SOFTWARE.
BaserK 0:9203a021a0be 26 *
BaserK 0:9203a021a0be 27 * @description of the program:
BaserK 0:9203a021a0be 28 *
BaserK 2:497faa1563ea 29 * First of all most of the credit goes to Kris Winer for his useful MPU6050 code.
BaserK 0:9203a021a0be 30 * I rewrite the code in my way using class prototypes and my comments. This program
BaserK 0:9203a021a0be 31 * can be a starter point for more advanced projects including quadcopters, balancing
BaserK 0:9203a021a0be 32 * robots etc. Program takes accelerometer and gyroscope data from the MPU6050 registers
BaserK 4:33fef1998fc8 33 * and calibrates them for better results. Then uses this data is to obtain pitch and roll
BaserK 4:33fef1998fc8 34 * angles and writes these angles to the terminal which mbed is connected to.
BaserK 0:9203a021a0be 35 *
BaserK 0:9203a021a0be 36 * @connections:
BaserK 0:9203a021a0be 37 *--------------------------------------------------------------
BaserK 0:9203a021a0be 38 * |LPC1768| |Peripherals|
BaserK 0:9203a021a0be 39 * Pin 9 ---------> SDA of MPU6050
BaserK 0:9203a021a0be 40 * Pin 10 --------> SCL of MPU6050
BaserK 4:33fef1998fc8 41 * GND -----------> GND of MPU6050
BaserK 0:9203a021a0be 42 * VOUT (3.3 V) --> VCC of MPU6050
BaserK 0:9203a021a0be 43 *---------------------------------------------------------------
BaserK 6:12ff524abb6a 44 *--------------------------------------------------------------
BaserK 6:12ff524abb6a 45 * |NUCLEO F411RE| |Peripherals|
BaserK 6:12ff524abb6a 46 * D14 -----------> SDA of MPU6050
BaserK 6:12ff524abb6a 47 * D15 -----------> SCL of MPU6050
BaserK 6:12ff524abb6a 48 * GND -----------> GND of MPU6050
BaserK 6:12ff524abb6a 49 * VOUT (3.3 V) --> VCC of MPU6050
BaserK 6:12ff524abb6a 50 *---------------------------------------------------------------
BaserK 6:12ff524abb6a 51
BaserK 6:12ff524abb6a 52
BaserK 0:9203a021a0be 53 * Note: For any mistakes or comments, please contact me.
BaserK 0:9203a021a0be 54 */
BaserK 0:9203a021a0be 55
BaserK 0:9203a021a0be 56 #include "mbed.h"
BaserK 0:9203a021a0be 57 #include "MPU6050.h"
BaserK 0:9203a021a0be 58 #include "ledControl.h"
mposter 13:e9fe7586ab0c 59 #include "USBMouse.h"
BaserK 0:9203a021a0be 60
mposter 13:e9fe7586ab0c 61 #include <deque> // std::queue
mposter 13:e9fe7586ab0c 62 #include <math.h>
mposter 13:e9fe7586ab0c 63
mposter 13:e9fe7586ab0c 64 #define QUEUE_LENGTH 5
mposter 13:e9fe7586ab0c 65 #define SCREEN_DISTANCE 0.5
mposter 13:e9fe7586ab0c 66 #define RESOLUTION 100
mposter 13:e9fe7586ab0c 67 #define ACCEL_RESOLUTION 500
mposter 13:e9fe7586ab0c 68 #define SCROLLUP_LENGTH 5
mposter 13:e9fe7586ab0c 69 #define SCROLLDOWN_LENGTH 5
remig 8:7b491d7b6d9d 70 //! Variable debug
remig 8:7b491d7b6d9d 71 #define DEBUG
BaserK 0:9203a021a0be 72 /* */
BaserK 0:9203a021a0be 73
BaserK 0:9203a021a0be 74 /* Defined in the MPU6050.cpp file */
mposter 10:72bbd203b210 75 //I2C i2c(p9,p10); // setup i2c (SDA,SCL)
BaserK 0:9203a021a0be 76
remig 8:7b491d7b6d9d 77 //! Instance pc de classe Serial
mposter 13:e9fe7586ab0c 78 //Serial pc(USBTX,USBRX); // default baud rate: 9600
remig 8:7b491d7b6d9d 79 //! Instance mpu6050 de classe MPU6050
BaserK 4:33fef1998fc8 80 MPU6050 mpu6050; // class: MPU6050, object: mpu6050
remig 8:7b491d7b6d9d 81 //! instance toggler1 de classe Ticker
remig 8:7b491d7b6d9d 82 //! instance filer de classe Ticker
remig 8:7b491d7b6d9d 83 // Ticker
remig 8:7b491d7b6d9d 84 // A Ticker is used to call a function at a recurring interval.
remig 8:7b491d7b6d9d 85 // You can use as many separate Ticker objects as you require.
BaserK 0:9203a021a0be 86 Ticker toggler1;
BaserK 3:88737ad5c803 87 Ticker filter;
BaserK 0:9203a021a0be 88
mposter 13:e9fe7586ab0c 89 //USBMouse objcet
mposter 13:e9fe7586ab0c 90 USBMouse mouse;
mposter 13:e9fe7586ab0c 91
BaserK 0:9203a021a0be 92 void toggle_led1();
BaserK 0:9203a021a0be 93 void toggle_led2();
BaserK 3:88737ad5c803 94 void compFilter();
BaserK 3:88737ad5c803 95
BaserK 3:88737ad5c803 96 float pitchAngle = 0;
BaserK 3:88737ad5c803 97 float rollAngle = 0;
BaserK 0:9203a021a0be 98
mposter 13:e9fe7586ab0c 99 //My funcs
mposter 13:e9fe7586ab0c 100 void calibrateGyro();
mposter 13:e9fe7586ab0c 101 void calibrateAccel();
mposter 13:e9fe7586ab0c 102 float sampleQueue(deque<float> q,float bias);
mposter 13:e9fe7586ab0c 103 void clearDeques();
mposter 13:e9fe7586ab0c 104 void updateDeques();
mposter 13:e9fe7586ab0c 105 int16_t processGX();
mposter 13:e9fe7586ab0c 106 int16_t processGZ();
mposter 13:e9fe7586ab0c 107 void processGyro();
mposter 13:e9fe7586ab0c 108 int16_t processAX();
mposter 13:e9fe7586ab0c 109 int16_t processAZ();
mposter 13:e9fe7586ab0c 110 void processAccel();
mposter 13:e9fe7586ab0c 111 void processMovement();
mposter 13:e9fe7586ab0c 112 void checkClicks();
mposter 13:e9fe7586ab0c 113 //end my funcs
mposter 13:e9fe7586ab0c 114
mposter 13:e9fe7586ab0c 115 //Create queue to insert gyroReadings into
mposter 13:e9fe7586ab0c 116 deque<float> gyroReadingsx;
mposter 13:e9fe7586ab0c 117 deque<float> gyroReadingsz;
mposter 13:e9fe7586ab0c 118
mposter 13:e9fe7586ab0c 119 //Create queue to insert accelReadings into
mposter 13:e9fe7586ab0c 120 deque<float> accelReadingsx;
mposter 13:e9fe7586ab0c 121 deque<float> accelReadingsz;
mposter 13:e9fe7586ab0c 122
mposter 13:e9fe7586ab0c 123 //Predefined gyro bias. Subtract these from queue averages to get movement weight
mposter 13:e9fe7586ab0c 124 float gyroBiasx = 0;
mposter 13:e9fe7586ab0c 125 float gyroBiasz = 0;
mposter 13:e9fe7586ab0c 126
mposter 13:e9fe7586ab0c 127 //Predefined accel bias. Subtract these from queue averages to get movement weight
mposter 13:e9fe7586ab0c 128 float accelBiasx = 0;
mposter 13:e9fe7586ab0c 129 float accelBiasz = 0;
mposter 13:e9fe7586ab0c 130
mposter 13:e9fe7586ab0c 131 //Clicking pins
mposter 13:e9fe7586ab0c 132 DigitalIn leftClick(p16);
mposter 13:e9fe7586ab0c 133 DigitalIn rightClick(p15);
mposter 13:e9fe7586ab0c 134
mposter 13:e9fe7586ab0c 135 //Scroll pins
mposter 13:e9fe7586ab0c 136 DigitalIn upScroll(p18);
mposter 13:e9fe7586ab0c 137 DigitalIn downScroll(p19);
mposter 13:e9fe7586ab0c 138
mposter 13:e9fe7586ab0c 139 //Pause pin
mposter 13:e9fe7586ab0c 140 DigitalIn pause(p20);
mposter 13:e9fe7586ab0c 141
BaserK 0:9203a021a0be 142 int main()
BaserK 0:9203a021a0be 143 {
mposter 13:e9fe7586ab0c 144 //Imported
mposter 13:e9fe7586ab0c 145 ////pc.baud(9600); // baud rate: 9600
BaserK 0:9203a021a0be 146 mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
BaserK 0:9203a021a0be 147 wait(1);
BaserK 0:9203a021a0be 148 mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
mposter 13:e9fe7586ab0c 149 ////////pc.printf("Calibration is completed. \r\n");
BaserK 0:9203a021a0be 150 wait(0.5);
BaserK 0:9203a021a0be 151 mpu6050.init(); // Initialize the sensor
BaserK 0:9203a021a0be 152 wait(1);
mposter 13:e9fe7586ab0c 153 ////////pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
BaserK 0:9203a021a0be 154 wait_ms(500);
mposter 13:e9fe7586ab0c 155 //End imported
mposter 13:e9fe7586ab0c 156
mposter 13:e9fe7586ab0c 157 //Perform our own sampling calibration
mposter 13:e9fe7586ab0c 158 for(int i = 0; i < QUEUE_LENGTH; i++){
mposter 13:e9fe7586ab0c 159 filter.attach(&compFilter, 0.005);
mposter 13:e9fe7586ab0c 160 updateDeques();
mposter 13:e9fe7586ab0c 161 }
mposter 13:e9fe7586ab0c 162 calibrateGyro();
mposter 13:e9fe7586ab0c 163 calibrateAccel();
mposter 13:e9fe7586ab0c 164
mposter 13:e9fe7586ab0c 165 ////////pc.printf("Bias x: %.3f | Bias z: %.3f\r\n",gyroBiasx,gyroBiasz);
mposter 13:e9fe7586ab0c 166
mposter 13:e9fe7586ab0c 167 //wait(2.5);
BaserK 0:9203a021a0be 168
BaserK 0:9203a021a0be 169 while(1)
BaserK 0:9203a021a0be 170 {
mposter 13:e9fe7586ab0c 171
mposter 13:e9fe7586ab0c 172
BaserK 4:33fef1998fc8 173
mposter 13:e9fe7586ab0c 174 //////////pc.printf(" _____________________________________________________________ \r\n");
mposter 13:e9fe7586ab0c 175 //////////pc.printf("| Accelerometer(g) | ax=%.3f | ay=%.3f | az=%.3f \r\n",ax,ay,az);
mposter 13:e9fe7586ab0c 176 printf("| Gyroscope(deg/s) | gx=%.3f | gy=%.3f | gz=%.3f \r\n",gx,gy,gz);
mposter 13:e9fe7586ab0c 177 //////////pc.printf("|_____________________________________________________________ \r\n\r\n");
mposter 13:e9fe7586ab0c 178 //
mposter 13:e9fe7586ab0c 179 //X AXIS GYRO
mposter 13:e9fe7586ab0c 180 //////pc.printf("x axis (gyro): ");
mposter 13:e9fe7586ab0c 181 //for(int i = 0; i < QUEUE_LENGTH; i++){
mposter 13:e9fe7586ab0c 182 //////////pc.printf("%.3f ",gyroReadingsx[i]);
mposter 13:e9fe7586ab0c 183 //}
mposter 13:e9fe7586ab0c 184 //////pc.printf("\r\n");
BaserK 0:9203a021a0be 185
mposter 13:e9fe7586ab0c 186 //Z AXIS GYRO
mposter 13:e9fe7586ab0c 187 //////pc.printf("z axis (gyro): ");
mposter 13:e9fe7586ab0c 188 //for(int i = 0; i < QUEUE_LENGTH; i++){
mposter 13:e9fe7586ab0c 189 //////////pc.printf("%.3f ",gyroReadingsz[i]);
mposter 13:e9fe7586ab0c 190 //}
mposter 13:e9fe7586ab0c 191 //////pc.printf("\r\n");
mposter 13:e9fe7586ab0c 192 //wait(2.5);
mposter 13:e9fe7586ab0c 193
mposter 13:e9fe7586ab0c 194
mposter 13:e9fe7586ab0c 195 filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
BaserK 0:9203a021a0be 196
mposter 13:e9fe7586ab0c 197 //#ifdef DEBUG
mposter 13:e9fe7586ab0c 198 //////////pc.printf(" [Debug On] \r\n");
mposter 13:e9fe7586ab0c 199 //mpu6050.complementaryFilter(&pitchAngle, &rollAngle);
mposter 13:e9fe7586ab0c 200 //#endif
mposter 13:e9fe7586ab0c 201 //////////pc.printf(" _______________\r\n");
mposter 13:e9fe7586ab0c 202 //////////pc.printf("| Pitch: %.3f \r\n",pitchAngle);
mposter 13:e9fe7586ab0c 203 //////////pc.printf("| Roll: %.3f \r\n",rollAngle);
mposter 13:e9fe7586ab0c 204 //////////pc.printf("|_______________\r\n\r\n");
mposter 13:e9fe7586ab0c 205
mposter 13:e9fe7586ab0c 206 //wait(1);
mposter 13:e9fe7586ab0c 207 updateDeques();
mposter 13:e9fe7586ab0c 208 processMovement();
mposter 13:e9fe7586ab0c 209 checkClicks();
mposter 13:e9fe7586ab0c 210
mposter 13:e9fe7586ab0c 211 //wait(2.5);
BaserK 0:9203a021a0be 212 }
BaserK 0:9203a021a0be 213 }
BaserK 0:9203a021a0be 214
mposter 13:e9fe7586ab0c 215 //Function to check and process button clicks for left and right mouse buttons
mposter 13:e9fe7586ab0c 216 void checkClicks(){
mposter 13:e9fe7586ab0c 217
mposter 13:e9fe7586ab0c 218 //Imported
mposter 13:e9fe7586ab0c 219 static bool preRightClick = false;
mposter 13:e9fe7586ab0c 220
mposter 13:e9fe7586ab0c 221 if (leftClick)
mposter 13:e9fe7586ab0c 222 {
mposter 13:e9fe7586ab0c 223 mouse.press(MOUSE_LEFT);
mposter 13:e9fe7586ab0c 224 }
mposter 13:e9fe7586ab0c 225 else
mposter 13:e9fe7586ab0c 226 {
mposter 13:e9fe7586ab0c 227 mouse.release(MOUSE_LEFT);
mposter 13:e9fe7586ab0c 228 }
mposter 13:e9fe7586ab0c 229
mposter 13:e9fe7586ab0c 230 // Right Mouse Click ___ Falling Edge Detection
mposter 13:e9fe7586ab0c 231 if (rightClick && !preRightClick)
mposter 13:e9fe7586ab0c 232 {
mposter 13:e9fe7586ab0c 233 preRightClick = true;
mposter 13:e9fe7586ab0c 234 }
mposter 13:e9fe7586ab0c 235 else if (!rightClick && preRightClick)
mposter 13:e9fe7586ab0c 236 { preRightClick = false;
mposter 13:e9fe7586ab0c 237 mouse.click(MOUSE_RIGHT);
mposter 13:e9fe7586ab0c 238 }
mposter 13:e9fe7586ab0c 239 //End imported
mposter 13:e9fe7586ab0c 240
mposter 13:e9fe7586ab0c 241 //Listen for scroll up button
mposter 13:e9fe7586ab0c 242 if(upScroll){
mposter 13:e9fe7586ab0c 243 for(int i = 0; i < SCROLLUP_LENGTH; i++){
mposter 13:e9fe7586ab0c 244 mouse.scroll(-1);
mposter 13:e9fe7586ab0c 245 }
mposter 13:e9fe7586ab0c 246 mouse.release(MOUSE_MIDDLE);
mposter 13:e9fe7586ab0c 247 }
mposter 13:e9fe7586ab0c 248
mposter 13:e9fe7586ab0c 249 //Listen for scroll down button
mposter 13:e9fe7586ab0c 250 if(downScroll){
mposter 13:e9fe7586ab0c 251 for(int i = 0; i < SCROLLDOWN_LENGTH; i++){
mposter 13:e9fe7586ab0c 252 mouse.scroll(1);
mposter 13:e9fe7586ab0c 253 }
mposter 13:e9fe7586ab0c 254 mouse.release(MOUSE_MIDDLE);
mposter 13:e9fe7586ab0c 255 }
mposter 13:e9fe7586ab0c 256
mposter 13:e9fe7586ab0c 257 //Loop while pause button is pressed
mposter 13:e9fe7586ab0c 258 while(pause);
mposter 13:e9fe7586ab0c 259 }
mposter 13:e9fe7586ab0c 260
mposter 13:e9fe7586ab0c 261 //Only care about x and z axis
mposter 13:e9fe7586ab0c 262 // -z -> Right side of screen; +z -> Left side of screen
mposter 13:e9fe7586ab0c 263 // -x - > Bottom of screen; +x -> Top of screen
mposter 13:e9fe7586ab0c 264
mposter 13:e9fe7586ab0c 265 int16_t processGX(){
mposter 13:e9fe7586ab0c 266
mposter 13:e9fe7586ab0c 267 //Obtain sample from deque
mposter 13:e9fe7586ab0c 268 float sample = sampleQueue(gyroReadingsx,gyroBiasx);
mposter 13:e9fe7586ab0c 269
mposter 13:e9fe7586ab0c 270 //Convert to radians
mposter 13:e9fe7586ab0c 271 float sampleRadians = sample*PI/180.0;
mposter 13:e9fe7586ab0c 272
mposter 13:e9fe7586ab0c 273 float deltaX = SCREEN_DISTANCE*RESOLUTION*tan(sampleRadians);
mposter 13:e9fe7586ab0c 274
mposter 13:e9fe7586ab0c 275 int16_t sending = 0 + deltaX;
mposter 13:e9fe7586ab0c 276
mposter 13:e9fe7586ab0c 277 printf("x axis MOVING: %.3f, sending: %i\r\n",deltaX,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(0,sending*-1);
mposter 13:e9fe7586ab0c 282
mposter 13:e9fe7586ab0c 283 return -1*sending;
mposter 13:e9fe7586ab0c 284 }
mposter 13:e9fe7586ab0c 285
mposter 13:e9fe7586ab0c 286 int16_t processGZ(){
mposter 13:e9fe7586ab0c 287
mposter 13:e9fe7586ab0c 288 //Obtain sample from deque
mposter 13:e9fe7586ab0c 289 float sample = sampleQueue(gyroReadingsz,gyroBiasz);
mposter 13:e9fe7586ab0c 290
mposter 13:e9fe7586ab0c 291 //////////////pc.printf("sample y: %.3f \r\n", sample);
mposter 13:e9fe7586ab0c 292
mposter 13:e9fe7586ab0c 293 //Convert to radians
mposter 13:e9fe7586ab0c 294 float sampleRadians = sample*PI/180.0;
mposter 13:e9fe7586ab0c 295
mposter 13:e9fe7586ab0c 296 //////pc.printf("sample (Radians) y: %.3f \r\n", sampleRadians);
mposter 13:e9fe7586ab0c 297
mposter 13:e9fe7586ab0c 298 float deltaZ = SCREEN_DISTANCE*RESOLUTION*tan(sampleRadians);
mposter 13:e9fe7586ab0c 299
mposter 13:e9fe7586ab0c 300 //Apply multipliers to enable mouse acceleration
mposter 13:e9fe7586ab0c 301 if(deltaZ > 0){
mposter 13:e9fe7586ab0c 302 deltaZ*=1.75;
mposter 13:e9fe7586ab0c 303 }else{
mposter 13:e9fe7586ab0c 304 deltaZ*=1.5;
mposter 13:e9fe7586ab0c 305 }
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 + deltaZ;
mposter 13:e9fe7586ab0c 310
mposter 13:e9fe7586ab0c 311 printf("z axis MOVING: %.3f, sending: %i\r\n",deltaZ,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 int16_t processAX(){
mposter 13:e9fe7586ab0c 320
mposter 13:e9fe7586ab0c 321 //Obtain sample from deque
mposter 13:e9fe7586ab0c 322 float sample = sampleQueue(accelReadingsx,accelBiasx);
mposter 13:e9fe7586ab0c 323
mposter 13:e9fe7586ab0c 324 int16_t sending = 0 + sample*ACCEL_RESOLUTION;
mposter 13:e9fe7586ab0c 325
mposter 13:e9fe7586ab0c 326 printf("ax axis MOVING: %.3f, sending: %i\r\n",sample,sending);
mposter 13:e9fe7586ab0c 327
mposter 13:e9fe7586ab0c 328 //Do tangent calculation to obtain opposite length
mposter 13:e9fe7586ab0c 329 ////x = L*tan(theta)
mposter 13:e9fe7586ab0c 330 //mouse.move(0,sending*-1);
mposter 13:e9fe7586ab0c 331 return -1*sending;
mposter 13:e9fe7586ab0c 332 }
mposter 13:e9fe7586ab0c 333
mposter 13:e9fe7586ab0c 334 int16_t processAZ(){
mposter 13:e9fe7586ab0c 335
mposter 13:e9fe7586ab0c 336 //Obtain sample from deque
mposter 13:e9fe7586ab0c 337 float sample = sampleQueue(accelReadingsz,accelBiasz);
mposter 13:e9fe7586ab0c 338
mposter 13:e9fe7586ab0c 339 //////////////pc.printf("sample y: %.3f \r\n", sample);
mposter 13:e9fe7586ab0c 340
mposter 13:e9fe7586ab0c 341 //////pc.printf("without cast: %.3f",SCREEN_DISTANCE*RESOLUTION*tan(sampleRadians));
mposter 13:e9fe7586ab0c 342
mposter 13:e9fe7586ab0c 343 int16_t sending = 0 + sample*ACCEL_RESOLUTION;
mposter 13:e9fe7586ab0c 344
mposter 13:e9fe7586ab0c 345 printf("az axis MOVING: %.3f, sending: %i\r\n",sample,sending);
mposter 13:e9fe7586ab0c 346
mposter 13:e9fe7586ab0c 347 //Do tangent calculation to obtain opposite length
mposter 13:e9fe7586ab0c 348 //x = L*tan(theta)
mposter 13:e9fe7586ab0c 349 //mouse.move(sending*-1,0);
mposter 13:e9fe7586ab0c 350 return -1*sending;
mposter 13:e9fe7586ab0c 351 }
mposter 13:e9fe7586ab0c 352
mposter 13:e9fe7586ab0c 353 void processGyro(){
mposter 13:e9fe7586ab0c 354 int16_t y = processGX();
mposter 13:e9fe7586ab0c 355 int16_t x = processGZ();
mposter 13:e9fe7586ab0c 356 mouse.move(x,y);
mposter 13:e9fe7586ab0c 357 }
mposter 13:e9fe7586ab0c 358
mposter 13:e9fe7586ab0c 359 void processAccel(){
mposter 13:e9fe7586ab0c 360 int16_t y = processAX();
mposter 13:e9fe7586ab0c 361 int16_t x = processAZ();
mposter 13:e9fe7586ab0c 362 mouse.move(x,y);
mposter 13:e9fe7586ab0c 363 }
mposter 13:e9fe7586ab0c 364
mposter 13:e9fe7586ab0c 365 void processMovement(){
mposter 13:e9fe7586ab0c 366 processGyro();
mposter 13:e9fe7586ab0c 367 //processAccel();
mposter 13:e9fe7586ab0c 368 }
mposter 13:e9fe7586ab0c 369
mposter 13:e9fe7586ab0c 370 void updateDeques(){
mposter 13:e9fe7586ab0c 371
mposter 13:e9fe7586ab0c 372 //Update gyro deques
mposter 13:e9fe7586ab0c 373 gyroReadingsx.pop_back();
mposter 13:e9fe7586ab0c 374 gyroReadingsz.pop_back();
mposter 13:e9fe7586ab0c 375 gyroReadingsx.push_front(gx);
mposter 13:e9fe7586ab0c 376 gyroReadingsz.push_front(gz);
mposter 13:e9fe7586ab0c 377
mposter 13:e9fe7586ab0c 378 //Update accel deques
mposter 13:e9fe7586ab0c 379 accelReadingsx.pop_back();
mposter 13:e9fe7586ab0c 380 accelReadingsz.pop_back();
mposter 13:e9fe7586ab0c 381 accelReadingsx.push_front(ax);
mposter 13:e9fe7586ab0c 382 accelReadingsz.push_front(az);
mposter 13:e9fe7586ab0c 383 }
mposter 13:e9fe7586ab0c 384
mposter 13:e9fe7586ab0c 385 void clearDeques(){
mposter 13:e9fe7586ab0c 386
mposter 13:e9fe7586ab0c 387 //Check for bad queue size
mposter 13:e9fe7586ab0c 388 if(gyroReadingsx.size() < QUEUE_LENGTH || gyroReadingsz.size() < QUEUE_LENGTH || accelReadingsx.size() < QUEUE_LENGTH || accelReadingsz.size() < QUEUE_LENGTH){
mposter 13:e9fe7586ab0c 389 while(1) printf("Bad queue size. Hanging...\n");
mposter 13:e9fe7586ab0c 390 }
mposter 13:e9fe7586ab0c 391
mposter 13:e9fe7586ab0c 392 //Clear gyro deques
mposter 13:e9fe7586ab0c 393 for(int i = 0; i < QUEUE_LENGTH; i++){
mposter 13:e9fe7586ab0c 394 gyroReadingsx.pop_back();
mposter 13:e9fe7586ab0c 395 gyroReadingsz.pop_back();
mposter 13:e9fe7586ab0c 396 }
mposter 13:e9fe7586ab0c 397
mposter 13:e9fe7586ab0c 398 //Clear accel deques
mposter 13:e9fe7586ab0c 399 for(int i = 0; i < QUEUE_LENGTH; i++){
mposter 13:e9fe7586ab0c 400 accelReadingsx.pop_back();
mposter 13:e9fe7586ab0c 401 accelReadingsz.pop_back();
mposter 13:e9fe7586ab0c 402 }
mposter 13:e9fe7586ab0c 403 }
mposter 13:e9fe7586ab0c 404
mposter 13:e9fe7586ab0c 405 void calibrateGyro(){
mposter 13:e9fe7586ab0c 406 //Only care about x and z axis
mposter 13:e9fe7586ab0c 407 // -z -> Right side of screen; +z -> Left side of screen
mposter 13:e9fe7586ab0c 408 // -x - > Bottom of screen; +x -> Top of screen
mposter 13:e9fe7586ab0c 409
mposter 13:e9fe7586ab0c 410 float xSum = 0;
mposter 13:e9fe7586ab0c 411 float zSum = 0;
mposter 13:e9fe7586ab0c 412
mposter 13:e9fe7586ab0c 413 //Iterate for QUEUE_LENGTH samples from gyroscope
mposter 13:e9fe7586ab0c 414 for(int i = 0; i < QUEUE_LENGTH; i++){
mposter 13:e9fe7586ab0c 415
mposter 13:e9fe7586ab0c 416 float xReading = gx;
mposter 13:e9fe7586ab0c 417 float zReading = gz;
mposter 13:e9fe7586ab0c 418
mposter 13:e9fe7586ab0c 419 printf("CALIB GX,GZ: %.3f %.3f \r\n", gx,gz);
mposter 13:e9fe7586ab0c 420
mposter 13:e9fe7586ab0c 421 //Apply filter to incoming readings
mposter 13:e9fe7586ab0c 422 filter.attach(&compFilter, 0.005);
mposter 13:e9fe7586ab0c 423
mposter 13:e9fe7586ab0c 424 //push gyro x and z readings onto deque
mposter 13:e9fe7586ab0c 425 gyroReadingsx.push_front(xReading);
mposter 13:e9fe7586ab0c 426 gyroReadingsz.push_front(zReading);
mposter 13:e9fe7586ab0c 427
mposter 13:e9fe7586ab0c 428 //add samples to sum totals
mposter 13:e9fe7586ab0c 429 xSum += xReading;
mposter 13:e9fe7586ab0c 430 zSum += zReading;
mposter 13:e9fe7586ab0c 431
mposter 13:e9fe7586ab0c 432 }
mposter 13:e9fe7586ab0c 433
mposter 13:e9fe7586ab0c 434 //Set sampled bias for each gyro axis
mposter 13:e9fe7586ab0c 435 gyroBiasx = xSum/QUEUE_LENGTH;
mposter 13:e9fe7586ab0c 436 gyroBiasz = zSum/QUEUE_LENGTH;
mposter 13:e9fe7586ab0c 437
mposter 13:e9fe7586ab0c 438 }
mposter 13:e9fe7586ab0c 439
mposter 13:e9fe7586ab0c 440 void calibrateAccel(){
mposter 13:e9fe7586ab0c 441 //Only care about x and z axis
mposter 13:e9fe7586ab0c 442 // -z -> Right side of screen; +z -> Left side of screen
mposter 13:e9fe7586ab0c 443 // -x - > Bottom of screen; +x -> Top of screen
mposter 13:e9fe7586ab0c 444
mposter 13:e9fe7586ab0c 445 float xSum = 0;
mposter 13:e9fe7586ab0c 446 float zSum = 0;
mposter 13:e9fe7586ab0c 447
mposter 13:e9fe7586ab0c 448 //Iterate for QUEUE_LENGTH samples from gyroscope
mposter 13:e9fe7586ab0c 449 for(int i = 0; i < QUEUE_LENGTH; i++){
mposter 13:e9fe7586ab0c 450
mposter 13:e9fe7586ab0c 451 float xReading = ax;
mposter 13:e9fe7586ab0c 452 float zReading = az;
mposter 13:e9fe7586ab0c 453
mposter 13:e9fe7586ab0c 454 printf("CALIB AX,AZ: %.3f %.3f \r\n", ax,az);
mposter 13:e9fe7586ab0c 455
mposter 13:e9fe7586ab0c 456 //Apply filter to incoming readings
mposter 13:e9fe7586ab0c 457 filter.attach(&compFilter, 0.005);
mposter 13:e9fe7586ab0c 458
mposter 13:e9fe7586ab0c 459 //push gyro x and z readings onto deque
mposter 13:e9fe7586ab0c 460 accelReadingsx.push_front(xReading);
mposter 13:e9fe7586ab0c 461 accelReadingsz.push_front(zReading);
mposter 13:e9fe7586ab0c 462
mposter 13:e9fe7586ab0c 463 //add samples to sum totals
mposter 13:e9fe7586ab0c 464 xSum += xReading;
mposter 13:e9fe7586ab0c 465 zSum += zReading;
mposter 13:e9fe7586ab0c 466
mposter 13:e9fe7586ab0c 467 }
mposter 13:e9fe7586ab0c 468
mposter 13:e9fe7586ab0c 469 //Set sampled bias for each gyro axis
mposter 13:e9fe7586ab0c 470 accelBiasx = xSum/QUEUE_LENGTH;
mposter 13:e9fe7586ab0c 471 accelBiasz = zSum/QUEUE_LENGTH;
mposter 13:e9fe7586ab0c 472
mposter 13:e9fe7586ab0c 473 }
mposter 13:e9fe7586ab0c 474
mposter 13:e9fe7586ab0c 475 //Calculate average of samples in a deque
mposter 13:e9fe7586ab0c 476 float calculateAverage(deque<float> q){
mposter 13:e9fe7586ab0c 477 float sum = 0;
mposter 13:e9fe7586ab0c 478 for(int i = 0; i < q.size(); i++){
mposter 13:e9fe7586ab0c 479 sum+=q[i];
mposter 13:e9fe7586ab0c 480 }
mposter 13:e9fe7586ab0c 481 return sum/q.size();
mposter 13:e9fe7586ab0c 482 }
mposter 13:e9fe7586ab0c 483
mposter 13:e9fe7586ab0c 484 //Subtract the bias from the average of samples in a deque to offset idle values
mposter 13:e9fe7586ab0c 485 float sampleQueue(deque<float> q,float bias){
mposter 13:e9fe7586ab0c 486
mposter 13:e9fe7586ab0c 487 //Alternatively, may want to calculate the average and subtract next incoming reading by avg
mposter 13:e9fe7586ab0c 488 return calculateAverage(q) - bias;
mposter 13:e9fe7586ab0c 489 }
mposter 13:e9fe7586ab0c 490
mposter 13:e9fe7586ab0c 491 //Imported
BaserK 0:9203a021a0be 492 void toggle_led1() {ledToggle(1);}
BaserK 0:9203a021a0be 493 void toggle_led2() {ledToggle(2);}
BaserK 3:88737ad5c803 494
BaserK 3:88737ad5c803 495 /* This function is created to avoid address error that caused from Ticker.attach func */
BaserK 4:33fef1998fc8 496 void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}
mposter 13:e9fe7586ab0c 497 //End imported