Diff: MyMPU6050.h

Revision:

7:07f1f975429a

Parent:

6:748bae310e1e

Child:

8:96fa92188874

--- a/MyMPU6050.h	Thu Dec 03 02:23:23 2015 +0000
+++ b/MyMPU6050.h	Thu Dec 03 23:55:33 2015 +0000
@@ -7,13 +7,13 @@
 
 #include "I2Cdev.h"
 #include "MPU6050_6Axis_MotionApps20.h"
-#include "CommDelegate.h"
+# define M_PI           3.14159265358979323846
 
-class MyMPU6050: public CommDelegate {
+class MyMPU6050 {
     
     public:
     
-        MyMPU6050(PinName i2cSda, PinName i2cScl) : mpu(i2cSda, i2cScl), checkpin(p11){
+        MyMPU6050(PinName i2cSda, PinName i2cScl, PinName interrupt) : mpu(i2cSda, i2cScl), checkpin(interrupt){
             this->setup(); 
         }
         
@@ -66,7 +66,12 @@
                 mpu.dmpGetGravity(&gravity, &q);
                 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
                 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
-                theta = ypr[2];
+                if (ypr[1]>0) theta = ypr[2];
+                else if (ypr[2]<0) theta = -M_PI-ypr[2];
+                else theta = M_PI-ypr[2];
+                
+//                theta = ypr[1];
+                
                 mpu.dmpGetGyro(gyroData, fifoBuffer);
                 dtheta = gyroData[2];
             }
@@ -80,12 +85,12 @@
             return theta;
         }
         
-        float getDtheta(){
+        float getDTheta(){
             return dtheta;
         }
 
     private:
-
+    
         MPU6050 mpu;
         
         bool dmpReady;  // set true if DMP init was successful
@@ -106,6 +111,7 @@
         InterruptIn checkpin;
         volatile bool mpuInterrupt;
         
+        
         void setup() {
             
             theta = 0;