Diff: main.cpp

Revision:

13:e9fe7586ab0c

Parent:

10:72bbd203b210

--- 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