Revision 6:748bae310e1e, committed 2015-12-03

Comitter:

amandaghassaei

Date:

Thu Dec 03 02:23:23 2015 +0000

Parent:

5:7d1bf3ce0053

Child:

7:07f1f975429a

Commit message:

delegate stuff working

Changed in this revision

--- a/I2Cdev.cpp	Sat Nov 23 16:47:00 2013 +0000
+++ b/I2Cdev.cpp	Thu Dec 03 02:23:23 2015 +0000
@@ -7,11 +7,6 @@
 
 #define useDebugSerial
 
-I2Cdev::I2Cdev(): i2c(I2C_SDA,I2C_SCL), debugSerial(USBTX, USBRX)
-{
-
-}
-
 I2Cdev::I2Cdev(PinName i2cSda, PinName i2cScl): i2c(i2cSda,i2cScl), debugSerial(USBTX, USBRX)
 {
 

--- a/I2Cdev.h	Sat Nov 23 16:47:00 2013 +0000
+++ b/I2Cdev.h	Thu Dec 03 02:23:23 2015 +0000
@@ -9,15 +9,11 @@
 
 #include "mbed.h"
 
-#define I2C_SDA p28
-#define I2C_SCL p27
-
 class I2Cdev {
     private:
         I2C i2c;
         Serial debugSerial;
     public:
-        I2Cdev();
         I2Cdev(PinName i2cSda, PinName i2cScl);        
         
         int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout());

--- a/MPU6050.cpp	Sat Nov 23 16:47:00 2013 +0000
+++ b/MPU6050.cpp	Thu Dec 03 02:23:23 2015 +0000
@@ -42,7 +42,7 @@
 
 #include "MPU6050.h"
 
-#define useDebugSerial
+//#define useDebugSerial
 
 //instead of using pgmspace.h
 typedef const unsigned char prog_uchar;
@@ -52,7 +52,7 @@
 /** Default constructor, uses default I2C address.
  * @see MPU6050_DEFAULT_ADDRESS
  */
-MPU6050::MPU6050() : debugSerial(USBTX, USBRX)
+MPU6050::MPU6050(PinName i2cSda, PinName i2cScl) : debugSerial(USBTX, USBRX), i2Cdev(i2cSda,i2cScl)
 {
     devAddr = MPU6050_DEFAULT_ADDRESS;
 }
@@ -63,7 +63,7 @@
  * @see MPU6050_ADDRESS_AD0_LOW
  * @see MPU6050_ADDRESS_AD0_HIGH
  */
-MPU6050::MPU6050(uint8_t address) : debugSerial(USBTX, USBRX)
+MPU6050::MPU6050(PinName i2cSda, PinName i2cScl, uint8_t address) : debugSerial(USBTX, USBRX), i2Cdev(i2cSda,i2cScl)
 {
     devAddr = address;
 }

--- a/MPU6050.h	Sat Nov 23 16:47:00 2013 +0000
+++ b/MPU6050.h	Thu Dec 03 02:23:23 2015 +0000
@@ -419,8 +419,8 @@
         I2Cdev i2Cdev;
         Serial debugSerial;
     public:
-        MPU6050();
-        MPU6050(uint8_t address);
+        MPU6050(PinName i2cSda, PinName i2cScl);
+        MPU6050(PinName i2cSda, PinName i2cScl, uint8_t address);
 
         void initialize();
         bool testConnection();

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MyMPU6050.h	Thu Dec 03 02:23:23 2015 +0000
@@ -0,0 +1,160 @@
+// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
+// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2012 Jeff Rowberg
+*/
+
+#include "I2Cdev.h"
+#include "MPU6050_6Axis_MotionApps20.h"
+#include "CommDelegate.h"
+
+class MyMPU6050: public CommDelegate {
+    
+    public:
+    
+        MyMPU6050(PinName i2cSda, PinName i2cScl) : mpu(i2cSda, i2cScl), checkpin(p11){
+            this->setup(); 
+        }
+        
+        void loop() {
+            // if programming failed, don't try to do anything
+            if (!dmpReady) return;
+        
+            // wait for MPU interrupt or extra packet(s) available
+            if (!mpuInterrupt && fifoCount < packetSize) {
+                return;
+                // 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;
+            mpuIntStatus = mpu.getIntStatus();
+        
+            // get current FIFO count
+            fifoCount = mpu.getFIFOCount();
+        
+            // check for overflow (this should never happen unless our code is too inefficient)
+            if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
+                // reset so we can continue cleanly
+                mpu.resetFIFO();
+                //Serial.println(F("FIFO overflow!"));
+        
+            // otherwise, check for DMP data ready interrupt (this should happen frequently)
+            } else if (mpuIntStatus & 0x02) {
+                // wait for correct available data length, should be a VERY short wait
+                while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
+        
+                // read a packet from FIFO
+                mpu.getFIFOBytes(fifoBuffer, packetSize);
+                
+                // track FIFO count here in case there is > 1 packet available
+                // (this lets us immediately read more without waiting for an interrupt)
+                fifoCount -= packetSize;
+        
+                // display Euler angles in degrees
+                mpu.dmpGetQuaternion(&q, fifoBuffer);
+                mpu.dmpGetGravity(&gravity, &q);
+                mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+                mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+                theta = ypr[2];
+                mpu.dmpGetGyro(gyroData, fifoBuffer);
+                dtheta = gyroData[2];
+            }
+        }
+        
+        void dmpDataReady() {
+            mpuInterrupt = true;
+        }
+        
+        float getTheta(){
+            return theta;
+        }
+        
+        float getDtheta(){
+            return dtheta;
+        }
+
+    private:
+
+        MPU6050 mpu;
+        
+        bool dmpReady;  // set true if DMP init was successful
+        uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
+        uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
+        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
+        
+        // orientation/motion vars
+        Quaternion q;           // [w, x, y, z]         quaternion container
+        VectorFloat gravity;    // [x, y, z]            gravity vector
+        float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
+        int16_t gyroData[3];
+        float theta;
+        float dtheta;
+    
+        InterruptIn checkpin;
+        volatile bool mpuInterrupt;
+        
+        void setup() {
+            
+            theta = 0;
+            dtheta = 0;
+            
+            dmpReady = false;            
+            mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
+        
+            // initialize device
+//            pc.printf("Initializing I2C devices...\r\n");
+            mpu.initialize();
+        
+            // verify connection
+//            pc.printf("Testing device connections...\r\n");
+            mpu.testConnection();//if() pc.printf("MPU6050 connection successful\r\n");
+//            else pc.printf("MPU6050 connection failed\r\n");
+        
+            // load and configure the DMP
+//            pc.printf("Initializing DMP...\r\n");
+            devStatus = mpu.dmpInitialize();
+            
+            // make sure it worked (returns 0 if so)
+            if (devStatus == 0) {
+                // turn on the DMP, now that it's ready
+//                pc.printf("Enabling DMP...\r\n");
+                mpu.setDMPEnabled(true);
+        
+                // enable Arduino interrupt detection
+//                pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
+                checkpin.rise(this, &MyMPU6050::dmpDataReady);
+                
+                mpuIntStatus = mpu.getIntStatus();
+        
+                // set our DMP Ready flag so the main loop() function knows it's okay to use it
+//                pc.printf("DMP ready! Waiting for first interrupt...\r\n");
+                dmpReady = true;
+        
+                // get expected DMP packet size for later comparison
+                packetSize = mpu.dmpGetFIFOPacketSize();
+            } else {
+                // ERROR!
+                // 1 = initial memory load failed
+                // 2 = DMP configuration updates failed
+                // (if it's going to break, usually the code will be 1)
+                
+//                pc.printf("DDMP Initialization failed (code ");
+//                pc.printf("%d", devStatus);
+//                pc.printf(")\r\n");
+            }
+        
+        }
+};
\ No newline at end of file