asdfsaf

Dependencies:   MPU6050-DMP-Ian mbed-rtos mbed

Files at this revision

API Documentation at this revision

Comitter:
majik
Date:
Tue May 27 09:23:23 2014 +0000
Commit message:
dfsasf

Changed in this revision

DMP.cpp Show annotated file Show diff for this revision Revisions of this file
DMP.h Show annotated file Show diff for this revision Revisions of this file
MPU6050-DMP-Ian.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed-rtos.lib Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
robot.cpp Show annotated file Show diff for this revision Revisions of this file
robot.h Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DMP.cpp	Tue May 27 09:23:23 2014 +0000
@@ -0,0 +1,205 @@
+//Copied MPU6050.h and MPU6050.cpp from another library (otherwise MPU gets stuck randomly)
+
+/*** THIS IS THE BEST VERSION OF DMP CODE
+    This code demonstrates how to access the MPU6050 IMU and fix the 
+    connection errors that occur, using a hardware hack.
+    It requires two hardware fixes: A transistor (PTE5) and an extra GPIO pin (PTE23) connected
+    to the SDA line.
+    Error 1: The SDA line gets stuck on ground.
+    Solution 1: Use the transistor to raise the SDA voltage off of ground. This resets the I2C bus. (line 71)
+    Error 2: The SDA line gets stuck on 3.3V
+    Solution 2: Use the GPIO line to pull the SDA voltage down a bit to reset the I2C bus. (line 96)
+    (I am currently trying to find a software solution for the above hack)
+    
+    This code works for using the DMP. (See "DMP" for implementation that can do raw accel data)
+    It appears all connection issues have been fixed.
+    Next step: make code neater (in progress)
+    
+    LEARNED: Do not need the PTE23 line. Only fix necessary is the transistor (PTE5)
+***/
+
+#include "mbed.h"
+#include "I2Cdev.h"
+#include "MPU6050_6Axis_MotionApps20.h"
+#include "robot.h"
+
+
+//******* MPU DECLARATION THINGS *****************//
+int dmp_test =1;    //if this is 0, get raw MPU values
+                 //if this is 1, get DMP values
+            
+int16_t ax, ay, az;
+int16_t gx, gy, gz;
+
+//******* DMP things ****************//
+// MPU control/status vars
+bool dmpReady = false;  // set true if DMP init was succesfful
+uint8_t mpuIntStatus;   // holds interrupt status byte from MPU
+uint8_t devStatus;      // return status after each device operation (0 = success)
+uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
+uint16_t fifoCount;     // count of all bytes currently in FIFO
+uint8_t fifoBuffer[64]; // FIFO storage buffer
+    
+    
+uint8_t MPU_CONNECTION;
+        
+// orientation/motion vars
+Quaternion q;           // [w,x,y,z]    quaternion container
+VectorInt16 aa;         // [x,y,z]      accel sensor measurements
+VectorInt16 aaReal;     // [x,y,z]      gravity-free accel sensor measurements
+VectorInt16 aaWorld;    // [x,y,z]      world-frame accel sensor measurements
+VectorFloat gravity;    // [x,y,z]      gravity vector
+float euler[3];         // [psi,theta,phi]  Euler angle container
+float ypr[3];           // [yaw,pitch,roll] yaw/pitch/roll container [rad]. Multiply by 180, divide by PI for [deg]
+InterruptIn checkpin(PTD7); //interrupt
+//*** Interrupt Detection Routine ***//
+volatile bool mpuInterrupt = false; //indicates whether interrupt pin has gone high
+void dmpDataReady(){
+    mpuInterrupt = true;
+}
+        
+//****** END MPU DECLARATION ********************//
+
+
+int test_dmp();    //this wraps up the code req'd to start the DMP
+void start_dmp(MPU6050 mpu);    //this will get the DMP ready
+void update_dmp(MPU6050 mpu);   //call this frequently
+
+//*********************FUNCTIONS***************************************************************//
+int test_dmp(){ //returns 1 if ok (right now, it doesn't return at all if there is a problem    
+    /// This function tests the I2C port for the MPU6050 and resets it if it is not working 
+    //NOTE: when the I2C connection is not successful, calling initialize() will cause the board to freeze
+    DigitalOut reset_pin(PTE5); //Transistor to pull up SDA line
+    reset_pin = 0;  //start the transistor in OFF mode
+    //int count = 0;
+    DigitalIn sample_1(PTE0); //(this pin will be used as the SDA pin when an MPU6050 object is declared
+    int temp = sample_1;    //Sample what is happening on the SDA line to guess what the solution is.
+    myled = 1;      //turn OFF LED. This will turn on again when the MPU is connected.
+    //bt.baud(baudRate);
+    DigitalOut reset_1(PTE0);//change the SDA pin to output.
+    reset_1 = 0;
+    if(temp == 1){  //if SDA line starts high, unstick by pulling it low
+        bt.printf("\n\rSetting up I2C connection...");
+        reset_1 = 0; //SDA pin = low
+        wait_ms(100);
+        MPU6050 *mpu2 = new MPU6050; //create a temporary MPU connection for testing
+        mpu2->reset();
+        //bt.baud(baudRate);
+        while(mpu2->testConnection() == 0){
+           delete mpu2; //delete mpu2 so I can use PTE0 as a GPIO again
+           bt.baud(baudRate); //you must reset baud rate whenever a new MPU6050 is declared
+           DigitalOut sda_pinx(PTE0);   
+           sda_pinx = 0;        //set this to 0 to pull the SDA line down from 3.3V
+           reset_pin = 0;       //make sure transistor = OFF
+           bt.printf("\n\rRetry");
+           wait_ms(50);
+           MPU6050 *mpu2 = new MPU6050;
+           wait_ms(500);
+           if(mpu2->testConnection() == 0)
+           {
+               reset_pin = 1;
+               wait_ms(50);
+               reset_pin = 0;
+           }
+        }
+        delete mpu2;    //free the memory allocated to mpu2 (this is important)
+    }else{
+        bt.printf("\n\rSetting up I2C connection...");
+        reset_1 = 1;
+        reset_pin = 1;
+        wait_ms(20);
+        reset_pin = 0;
+        reset_1 = 0;
+        wait_ms(20);
+    }
+    reset_pin = 0;
+    DigitalIn not_used(PTE23);
+    myled = 0;  //turn ON LED
+    return 1;
+}
+
+
+void start_dmp(MPU6050 mpu1){    //this will get the DMP ready
+    //initialize device
+    mpu1.reset();
+    bt.printf("\n\rInitializing I2C devices...");
+    devStatus = mpu1.dmpInitialize();
+    //insert gyro offsets here// Write a calibration algorithm in the future
+    mpu1.setXGyroOffset(-31); //800/25=32 //-31 is the largest offset available
+    mpu1.setYGyroOffset(-183/25);
+    mpu1.setZGyroOffset(80/25);
+    mpu1.setXAccelOffset(0);     //the accel offsets don't do anything
+    mpu1.setYAccelOffset(0);
+    mpu1.setZAccelOffset(0);
+    
+    
+    if(devStatus == 0){     //this means initialize was successful
+        //turn on DMP
+        bt.printf("\n\rEnabling DMP");
+        mpu1.setDMPEnabled(true);
+        
+        // enable  interrupt detection
+        bt.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
+        checkpin.rise(&dmpDataReady);
+    
+        mpuIntStatus = mpu1.getIntStatus();
+        
+        //set the DMP ready flag so main loop knows when it is ok to use
+        bt.printf("DMP ready! Waiting for first interrupt");
+        dmpReady = true;
+        
+        //get expected DMP packet size
+        packetSize = mpu1.dmpGetFIFOPacketSize();
+    }else{
+        bt.printf("\n\rDMP Initialization failed");
+        bt.printf("\t%d",devStatus);
+        wait_ms(1000);
+    }
+}
+void update_dmp(MPU6050 mpu1){
+    
+    if (!dmpReady) return;
+    //while (!mpuInterrupt && fifoCount < packetSize) {
+        // other program behavior stuff here
+        // if you are really paranoid you can frequently test in between other
+        // stuff to see if mpuInterrupt is true, and if so, "break;" from the
+        // while() loop to immediately process the MPU data
+        // .
+    //}
+    // reset interrupt flag and get INT_STATUS byte
+    //mpuInterrupt = false;   //this resets the interrupt flag
+    mpuIntStatus = mpu1.getIntStatus();
+    
+    //get current FIFO count;
+    fifoCount = mpu1.getFIFOCount();
+    
+    //check for overflow (only happens if your code sucks)
+    if((mpuIntStatus & 0x10) || fifoCount == 1024){
+        //reset so we can continue cleanly
+        mpu1.resetFIFO();
+        bt.printf("\n\rFIFO overflow");
+    } else if (mpuIntStatus & 0x02){
+        //wait for correct available data length (should be very short)
+        while (fifoCount < packetSize) fifoCount = mpu1.getFIFOCount();
+
+        //read a packet from FIFO
+        mpu1.getFIFOBytes(fifoBuffer, packetSize);
+        
+        //track FIFO count here in the case there is > 1 packet available
+        //(allows us to immediately read more w/o waiting for interrupt)
+        fifoCount -= packetSize;
+        
+        mpu1.dmpGetQuaternion(&q,fifoBuffer);
+        mpu1.dmpGetGravity(&gravity, &q);
+        mpu1.dmpGetYawPitchRoll(ypr,&q,&gravity);
+        #ifdef PRINT_GYR
+        bt.printf("\n\rypr\t %f\t %f\t %f",ypr[0]*180/PI,ypr[1]*180/PI,ypr[2]*180/PI);
+        #endif
+        
+        mpu1.dmpGetAccel(&aa, fifoBuffer);
+        mpu1.dmpGetLinearAccel(&aaReal, &aa, &gravity);
+        #ifdef PRINT_ACC
+        bt.printf("\n\rareal\t%d\t%d\t%d",aaReal.x,aaReal.y,aaReal.z);
+        #endif
+   }     
+}
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DMP.h	Tue May 27 09:23:23 2014 +0000
@@ -0,0 +1,12 @@
+#ifndef DMP_H
+#define DMP_H
+
+extern volatile bool mpuInterrupt; //indicates whether interrupt pin has gone high
+extern uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
+extern uint16_t fifoCount;     // count of all bytes currently in FIFO
+
+int test_dmp();
+void start_dmp(MPU6050);    //this will get the DMP ready
+void update_dmp(MPU6050);   //call this frequently
+
+#endif
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050-DMP-Ian.lib	Tue May 27 09:23:23 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/majik/code/MPU6050-DMP-Ian/#3f0bc381e1dd
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue May 27 09:23:23 2014 +0000
@@ -0,0 +1,44 @@
+/*
+    This code is in working condition.
+    DMP works in RTOS environment
+*/
+
+#include "mbed.h"
+#include "rtos.h"
+#include "robot.h"
+#include "MPU6050_6Axis_MotionApps20.h"
+#include "DMP.h"
+
+
+
+//int main() {
+void led2_thread(void const *args){
+    DigitalOut reset_pin(PTE5);
+    test_dmp();
+    MPU6050 mpu;
+   
+    myled = 0;   //turn on LED
+
+    btSwitch = 1;
+    bt.baud(baudRate);   //set baud rate for bluetooth device
+    
+    start_dmp(mpu);
+    
+    while(1){
+        //bt.baud(baudRate); 
+        //reset_pin = 0;
+        if(!mpuInterrupt && fifoCount < packetSize);    //interrupt not ready
+        else{   //mpu interrupt is ready
+            update_dmp(mpu);
+            mpuInterrupt = false;   //this resets the interrupt flag
+        }       
+    }
+}
+
+int main() {
+    Thread thread(led2_thread);
+    
+    while (true) {
+        Thread::wait(500);
+    }
+}
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Tue May 27 09:23:23 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#3761f69dbbb2
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue May 27 09:23:23 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/5e5da4a5990b
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/robot.cpp	Tue May 27 09:23:23 2014 +0000
@@ -0,0 +1,5 @@
+#include "mbed.h"
+
+DigitalOut myled(PTE3);
+Serial bt(PTA2,PTA1);
+DigitalOut btSwitch(PTE25);
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/robot.h	Tue May 27 09:23:23 2014 +0000
@@ -0,0 +1,13 @@
+#ifndef ROBOT_H
+#define ROBOT_H
+
+#define baudRate 115200     //this is the baud rate of the bluetooth device (you just have to know this)
+#define PI 3.1415926
+#define PRINT_GYR     //uncomment this to print gyro data
+//#define PRINT_ACC     //uncomment this to print accel data
+
+extern DigitalOut myled;
+extern Serial bt;
+extern DigitalOut btSwitch;
+
+#endif
\ No newline at end of file