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