Revision 14:e6e3099e3b3b, committed 2020-12-02

Comitter:

mposter

Date:

Wed Dec 02 00:19:20 2020 +0000

Parent:

13:e9fe7586ab0c

Commit message:

Mini cleanup

Changed in this revision

--- a/main.cpp	Mon Nov 30 16:39:57 2020 +0000
+++ b/main.cpp	Wed Dec 02 00:19:20 2020 +0000
@@ -1,37 +1,4 @@
-/*   Getting Pitch and Roll Angles from MPU6050
-*
-*    @author: Baser Kandehir 
-*    @date: July 16, 2015
-*    @license: MIT license
-*     
-*   Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr
-*
-*   Permission is hereby granted, free of charge, to any person obtaining a copy
-*   of this software and associated documentation files (the "Software"), to deal
-*   in the Software without restriction, including without limitation the rights
-*   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-*   copies of the Software, and to permit persons to whom the Software is
-*   furnished to do so, subject to the following conditions:
-*
-*   The above copyright notice and this permission notice shall be included in
-*   all copies or substantial portions of the Software.
-*
-*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-*   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-*   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-*   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-*   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-*   THE SOFTWARE.
-*   
-*    @description of the program: 
-*    
-*    First of all most of the credit goes to Kris Winer for his useful MPU6050 code.
-*    I rewrite the code in my way using class prototypes and my comments. This program
-*    can be a starter point for more advanced projects including quadcopters, balancing
-*    robots etc. Program takes accelerometer and gyroscope data from the MPU6050 registers
-*    and calibrates them for better results. Then uses this data is to obtain pitch and roll
-*    angles and writes these angles to the terminal which mbed is connected to. 
+/*
 *   
 *    @connections:
 *-------------------------------------------------------------- 
@@ -48,9 +15,6 @@
 *    GND -----------> GND of MPU6050
 *    VOUT (3.3 V) --> VCC of MPU6050
 *---------------------------------------------------------------
-
-
-*   Note: For any mistakes or comments, please contact me.
 */
 
 #include "mbed.h"
@@ -69,17 +33,13 @@
 #define SCROLLDOWN_LENGTH 5
 //! Variable debug
 #define DEBUG
-/* */
-
-/* Defined in the MPU6050.cpp file  */
- //I2C i2c(p9,p10);         // setup i2c (SDA,SCL)  
+/* */ 
 
 //! Instance pc de classe Serial
 //Serial pc(USBTX,USBRX);    // default baud rate: 9600
 //! Instance mpu6050 de classe MPU6050
 MPU6050 mpu6050;           // class: MPU6050, object: mpu6050 
-//! instance toggler1 de classe Ticker
-//! instance filer de classe Ticker
+
 // Ticker 
 // A Ticker is used to call a function at a recurring interval.
 // You can use as many separate Ticker objects as you require.
@@ -93,6 +53,7 @@
 void toggle_led2();
 void compFilter();
 
+//Not used by us, but required to apply the filter from the MPU lib
 float pitchAngle = 0;
 float rollAngle = 0;
 
@@ -141,7 +102,10 @@
 
 int main() 
 {
-    //Imported
+    
+    //Begin initialize*********************************************************
+    
+    //Imported https://os.mbed.com/users/remig/code/i2c_MPU6050//file/369106aa267a/main.cpp/
     ////pc.baud(9600);                              // baud rate: 9600
     mpu6050.whoAmI();                           // Communication test: WHO_AM_I register reading 
     wait(1);
@@ -154,14 +118,20 @@
     wait_ms(500);
     //End imported
     
+    
+    
     //Perform our own sampling calibration
     for(int i = 0; i < QUEUE_LENGTH; i++){
         filter.attach(&compFilter, 0.005);   
         updateDeques();
     }
+    
+    //Calculate biases
     calibrateGyro();
     calibrateAccel();
     
+    //End intialize***********************************************************
+    
     ////////pc.printf("Bias x: %.3f | Bias z: %.3f\r\n",gyroBiasx,gyroBiasz);
     
     //wait(2.5);
@@ -173,7 +143,7 @@
         
         //////////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);
+        //printf("| Gyroscope(deg/s) | gx=%.3f | gy=%.3f | gz=%.3f                \r\n",gx,gy,gz);
         //////////pc.printf("|_____________________________________________________________  \r\n\r\n");
 //      
         //X AXIS GYRO
@@ -194,21 +164,11 @@
         
         filter.attach(&compFilter, 0.005);    // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
         
-        //#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);
+        //wait(2.5);
     }
 }
 
@@ -216,6 +176,7 @@
 void checkClicks(){
     
     //Imported
+    //https://os.mbed.com/users/guqinchen/code/P4_IMU//file/044740372a78/main.cpp/
     static bool preRightClick = false;  
  
      if (leftClick)   
@@ -270,8 +231,10 @@
    //Convert to radians
    float sampleRadians = sample*PI/180.0;
    
+   //Compute distance to move
    float deltaX = SCREEN_DISTANCE*RESOLUTION*tan(sampleRadians);
    
+   //Cast to int16_t
    int16_t sending = 0 + deltaX;
    
    printf("x axis MOVING: %.3f, sending: %i\r\n",deltaX,sending);
@@ -295,12 +258,15 @@
    
    //////pc.printf("sample (Radians) y: %.3f \r\n", sampleRadians);
    
+    //Compute distance to move
    float deltaZ = SCREEN_DISTANCE*RESOLUTION*tan(sampleRadians);
    
    //Apply multipliers to enable mouse acceleration
    if(deltaZ > 0){
         deltaZ*=1.75;       
     }else{
+        //If the amount to move z was negative, it tended to be stronger than the amount to move the z in the positive
+        //  so we weighted negative z deltas less than positive ones
         deltaZ*=1.5;
     }