Final project for CS335 By Maxwell Poster and Jeffrey Resnik
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
- Child:
- 14:e6e3099e3b3b
final
Who changed what in which revision?
User | Revision | Line number | New 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 |