Diff: MyMPU6050.h

Revision:

6:748bae310e1e

Child:

7:07f1f975429a

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