Final project for CS335 By Maxwell Poster and Jeffrey Resnik
Dependencies: mbed MPU6050_template ledControl2 USBDevice
Diff: main.cpp
- Revision:
- 13:e9fe7586ab0c
- Parent:
- 10:72bbd203b210
- Child:
- 14:e6e3099e3b3b
--- a/main.cpp Sun Nov 29 03:38:43 2020 +0000 +++ b/main.cpp Mon Nov 30 16:39:57 2020 +0000 @@ -56,7 +56,17 @@ #include "mbed.h" #include "MPU6050.h" #include "ledControl.h" +#include "USBMouse.h" +#include <deque> // std::queue +#include <math.h> + +#define QUEUE_LENGTH 5 +#define SCREEN_DISTANCE 0.5 +#define RESOLUTION 100 +#define ACCEL_RESOLUTION 500 +#define SCROLLUP_LENGTH 5 +#define SCROLLDOWN_LENGTH 5 //! Variable debug #define DEBUG /* */ @@ -65,7 +75,7 @@ //I2C i2c(p9,p10); // setup i2c (SDA,SCL) //! Instance pc de classe Serial -Serial pc(USBTX,USBRX); // default baud rate: 9600 +//Serial pc(USBTX,USBRX); // default baud rate: 9600 //! Instance mpu6050 de classe MPU6050 MPU6050 mpu6050; // class: MPU6050, object: mpu6050 //! instance toggler1 de classe Ticker @@ -76,6 +86,9 @@ Ticker toggler1; Ticker filter; +//USBMouse objcet +USBMouse mouse; + void toggle_led1(); void toggle_led2(); void compFilter(); @@ -83,49 +96,402 @@ float pitchAngle = 0; float rollAngle = 0; +//My funcs +void calibrateGyro(); +void calibrateAccel(); +float sampleQueue(deque<float> q,float bias); +void clearDeques(); +void updateDeques(); +int16_t processGX(); +int16_t processGZ(); +void processGyro(); +int16_t processAX(); +int16_t processAZ(); +void processAccel(); +void processMovement(); +void checkClicks(); +//end my funcs + +//Create queue to insert gyroReadings into +deque<float> gyroReadingsx; +deque<float> gyroReadingsz; + +//Create queue to insert accelReadings into +deque<float> accelReadingsx; +deque<float> accelReadingsz; + +//Predefined gyro bias. Subtract these from queue averages to get movement weight +float gyroBiasx = 0; +float gyroBiasz = 0; + +//Predefined accel bias. Subtract these from queue averages to get movement weight +float accelBiasx = 0; +float accelBiasz = 0; + +//Clicking pins +DigitalIn leftClick(p16); +DigitalIn rightClick(p15); + +//Scroll pins +DigitalIn upScroll(p18); +DigitalIn downScroll(p19); + +//Pause pin +DigitalIn pause(p20); + int main() { - pc.baud(9600); // baud rate: 9600 + //Imported + ////pc.baud(9600); // baud rate: 9600 mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading wait(1); mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers - pc.printf("Calibration is completed. \r\n"); + ////////pc.printf("Calibration is completed. \r\n"); wait(0.5); mpu6050.init(); // Initialize the sensor wait(1); - pc.printf("MPU6050 is initialized for operation.. \r\n\r\n"); + ////////pc.printf("MPU6050 is initialized for operation.. \r\n\r\n"); wait_ms(500); + //End imported + + //Perform our own sampling calibration + for(int i = 0; i < QUEUE_LENGTH; i++){ + filter.attach(&compFilter, 0.005); + updateDeques(); + } + calibrateGyro(); + calibrateAccel(); + + ////////pc.printf("Bias x: %.3f | Bias z: %.3f\r\n",gyroBiasx,gyroBiasz); + + //wait(2.5); while(1) { - - /* Uncomment below if you want to see accel and gyro data */ + + - pc.printf(" _____________________________________________________________ \r\n"); - pc.printf("| Accelerometer(g) | ax=%.3f | ay=%.3f | az=%.3f \r\n",ax,ay,az); - pc.printf("| Gyroscope(deg/s) | gx=%.3f | gy=%.3f | gz=%.3f \r\n",gx,gy,gz); - pc.printf("|_____________________________________________________________ \r\n\r\n"); -// - wait(2.5); - - //filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period) + //////////pc.printf(" _____________________________________________________________ \r\n"); + //////////pc.printf("| Accelerometer(g) | ax=%.3f | ay=%.3f | az=%.3f \r\n",ax,ay,az); + printf("| Gyroscope(deg/s) | gx=%.3f | gy=%.3f | gz=%.3f \r\n",gx,gy,gz); + //////////pc.printf("|_____________________________________________________________ \r\n\r\n"); +// + //X AXIS GYRO + //////pc.printf("x axis (gyro): "); + //for(int i = 0; i < QUEUE_LENGTH; i++){ + //////////pc.printf("%.3f ",gyroReadingsx[i]); + //} + //////pc.printf("\r\n"); - #ifdef DEBUG - pc.printf(" [Debug On] \r\n"); - mpu6050.complementaryFilter(&pitchAngle, &rollAngle); - #endif - pc.printf(" _______________\r\n"); - pc.printf("| Pitch: %.3f \r\n",pitchAngle); - pc.printf("| Roll: %.3f \r\n",rollAngle); - pc.printf("|_______________\r\n\r\n"); + //Z AXIS GYRO + //////pc.printf("z axis (gyro): "); + //for(int i = 0; i < QUEUE_LENGTH; i++){ + //////////pc.printf("%.3f ",gyroReadingsz[i]); + //} + //////pc.printf("\r\n"); + //wait(2.5); + + + filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period) - wait(1); - + //#ifdef DEBUG + //////////pc.printf(" [Debug On] \r\n"); + //mpu6050.complementaryFilter(&pitchAngle, &rollAngle); + //#endif + //////////pc.printf(" _______________\r\n"); + //////////pc.printf("| Pitch: %.3f \r\n",pitchAngle); + //////////pc.printf("| Roll: %.3f \r\n",rollAngle); + //////////pc.printf("|_______________\r\n\r\n"); + + //wait(1); + updateDeques(); + processMovement(); + checkClicks(); + + //wait(2.5); } } +//Function to check and process button clicks for left and right mouse buttons +void checkClicks(){ + + //Imported + static bool preRightClick = false; + + if (leftClick) + { + mouse.press(MOUSE_LEFT); + } + else + { + mouse.release(MOUSE_LEFT); + } + + // Right Mouse Click ___ Falling Edge Detection + if (rightClick && !preRightClick) + { + preRightClick = true; + } + else if (!rightClick && preRightClick) + { preRightClick = false; + mouse.click(MOUSE_RIGHT); + } + //End imported + + //Listen for scroll up button + if(upScroll){ + for(int i = 0; i < SCROLLUP_LENGTH; i++){ + mouse.scroll(-1); + } + mouse.release(MOUSE_MIDDLE); + } + + //Listen for scroll down button + if(downScroll){ + for(int i = 0; i < SCROLLDOWN_LENGTH; i++){ + mouse.scroll(1); + } + mouse.release(MOUSE_MIDDLE); + } + + //Loop while pause button is pressed + while(pause); +} + +//Only care about x and z axis + // -z -> Right side of screen; +z -> Left side of screen + // -x - > Bottom of screen; +x -> Top of screen + +int16_t processGX(){ + + //Obtain sample from deque + float sample = sampleQueue(gyroReadingsx,gyroBiasx); + + //Convert to radians + float sampleRadians = sample*PI/180.0; + + float deltaX = SCREEN_DISTANCE*RESOLUTION*tan(sampleRadians); + + int16_t sending = 0 + deltaX; + + printf("x axis MOVING: %.3f, sending: %i\r\n",deltaX,sending); + + //Do tangent calculation to obtain opposite length + //x = L*tan(theta) + //mouse.move(0,sending*-1); + + return -1*sending; +} + +int16_t processGZ(){ + + //Obtain sample from deque + float sample = sampleQueue(gyroReadingsz,gyroBiasz); + + //////////////pc.printf("sample y: %.3f \r\n", sample); + + //Convert to radians + float sampleRadians = sample*PI/180.0; + + //////pc.printf("sample (Radians) y: %.3f \r\n", sampleRadians); + + float deltaZ = SCREEN_DISTANCE*RESOLUTION*tan(sampleRadians); + + //Apply multipliers to enable mouse acceleration + if(deltaZ > 0){ + deltaZ*=1.75; + }else{ + deltaZ*=1.5; + } + + //////pc.printf("without cast: %.3f",SCREEN_DISTANCE*RESOLUTION*tan(sampleRadians)); + + int16_t sending = 0 + deltaZ; + + printf("z axis MOVING: %.3f, sending: %i\r\n",deltaZ,sending); + + //Do tangent calculation to obtain opposite length + //x = L*tan(theta) + //mouse.move(sending*-1,0); + return -1*sending; +} + +int16_t processAX(){ + + //Obtain sample from deque + float sample = sampleQueue(accelReadingsx,accelBiasx); + + int16_t sending = 0 + sample*ACCEL_RESOLUTION; + + printf("ax axis MOVING: %.3f, sending: %i\r\n",sample,sending); + + //Do tangent calculation to obtain opposite length + ////x = L*tan(theta) + //mouse.move(0,sending*-1); + return -1*sending; +} + +int16_t processAZ(){ + + //Obtain sample from deque + float sample = sampleQueue(accelReadingsz,accelBiasz); + + //////////////pc.printf("sample y: %.3f \r\n", sample); + + //////pc.printf("without cast: %.3f",SCREEN_DISTANCE*RESOLUTION*tan(sampleRadians)); + + int16_t sending = 0 + sample*ACCEL_RESOLUTION; + + printf("az axis MOVING: %.3f, sending: %i\r\n",sample,sending); + + //Do tangent calculation to obtain opposite length + //x = L*tan(theta) + //mouse.move(sending*-1,0); + return -1*sending; +} + +void processGyro(){ + int16_t y = processGX(); + int16_t x = processGZ(); + mouse.move(x,y); +} + +void processAccel(){ + int16_t y = processAX(); + int16_t x = processAZ(); + mouse.move(x,y); +} + +void processMovement(){ + processGyro(); + //processAccel(); +} + +void updateDeques(){ + + //Update gyro deques + gyroReadingsx.pop_back(); + gyroReadingsz.pop_back(); + gyroReadingsx.push_front(gx); + gyroReadingsz.push_front(gz); + + //Update accel deques + accelReadingsx.pop_back(); + accelReadingsz.pop_back(); + accelReadingsx.push_front(ax); + accelReadingsz.push_front(az); +} + +void clearDeques(){ + + //Check for bad queue size + if(gyroReadingsx.size() < QUEUE_LENGTH || gyroReadingsz.size() < QUEUE_LENGTH || accelReadingsx.size() < QUEUE_LENGTH || accelReadingsz.size() < QUEUE_LENGTH){ + while(1) printf("Bad queue size. Hanging...\n"); + } + + //Clear gyro deques + for(int i = 0; i < QUEUE_LENGTH; i++){ + gyroReadingsx.pop_back(); + gyroReadingsz.pop_back(); + } + + //Clear accel deques + for(int i = 0; i < QUEUE_LENGTH; i++){ + accelReadingsx.pop_back(); + accelReadingsz.pop_back(); + } +} + +void calibrateGyro(){ + //Only care about x and z axis + // -z -> Right side of screen; +z -> Left side of screen + // -x - > Bottom of screen; +x -> Top of screen + + float xSum = 0; + float zSum = 0; + + //Iterate for QUEUE_LENGTH samples from gyroscope + for(int i = 0; i < QUEUE_LENGTH; i++){ + + float xReading = gx; + float zReading = gz; + + printf("CALIB GX,GZ: %.3f %.3f \r\n", gx,gz); + + //Apply filter to incoming readings + filter.attach(&compFilter, 0.005); + + //push gyro x and z readings onto deque + gyroReadingsx.push_front(xReading); + gyroReadingsz.push_front(zReading); + + //add samples to sum totals + xSum += xReading; + zSum += zReading; + + } + + //Set sampled bias for each gyro axis + gyroBiasx = xSum/QUEUE_LENGTH; + gyroBiasz = zSum/QUEUE_LENGTH; + +} + +void calibrateAccel(){ + //Only care about x and z axis + // -z -> Right side of screen; +z -> Left side of screen + // -x - > Bottom of screen; +x -> Top of screen + + float xSum = 0; + float zSum = 0; + + //Iterate for QUEUE_LENGTH samples from gyroscope + for(int i = 0; i < QUEUE_LENGTH; i++){ + + float xReading = ax; + float zReading = az; + + printf("CALIB AX,AZ: %.3f %.3f \r\n", ax,az); + + //Apply filter to incoming readings + filter.attach(&compFilter, 0.005); + + //push gyro x and z readings onto deque + accelReadingsx.push_front(xReading); + accelReadingsz.push_front(zReading); + + //add samples to sum totals + xSum += xReading; + zSum += zReading; + + } + + //Set sampled bias for each gyro axis + accelBiasx = xSum/QUEUE_LENGTH; + accelBiasz = zSum/QUEUE_LENGTH; + +} + +//Calculate average of samples in a deque +float calculateAverage(deque<float> q){ + float sum = 0; + for(int i = 0; i < q.size(); i++){ + sum+=q[i]; + } + return sum/q.size(); +} + +//Subtract the bias from the average of samples in a deque to offset idle values +float sampleQueue(deque<float> q,float bias){ + + //Alternatively, may want to calculate the average and subtract next incoming reading by avg + return calculateAverage(q) - bias; +} + +//Imported void toggle_led1() {ledToggle(1);} void toggle_led2() {ledToggle(2);} /* This function is created to avoid address error that caused from Ticker.attach func */ void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);} +//End imported \ No newline at end of file