File content as of revision 13:e9fe7586ab0c:

/*   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:
*-------------------------------------------------------------- 
*    |LPC1768|        |Peripherals|
*    Pin 9 ---------> SDA of MPU6050
*    Pin 10 --------> SCL of MPU6050
*    GND -----------> GND of MPU6050
*    VOUT (3.3 V) --> VCC of MPU6050
*---------------------------------------------------------------
*-------------------------------------------------------------- 
*    |NUCLEO F411RE|  |Peripherals|
*    D14 -----------> SDA of MPU6050
*    D15 -----------> SCL of MPU6050
*    GND -----------> GND of MPU6050
*    VOUT (3.3 V) --> VCC of MPU6050
*---------------------------------------------------------------


*   Note: For any mistakes or comments, please contact me.
*/

#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
/* */

/* 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.
Ticker toggler1;
Ticker filter;           

//USBMouse objcet
USBMouse mouse;

void toggle_led1();
void toggle_led2();
void compFilter();

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() 
{
    //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");
    wait(0.5);
    mpu6050.init();                             // Initialize the sensor
    wait(1);
    ////////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) 
    {
        
        
        
        //////////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");
        
        //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)
        
        //#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