Diff: MyMPU6050.h

Revision:

8:96fa92188874

Parent:

7:07f1f975429a

Child:

9:ac76b5b24b18

--- a/MyMPU6050.h	Thu Dec 03 23:55:33 2015 +0000
+++ b/MyMPU6050.h	Fri Dec 04 07:10:43 2015 +0000
@@ -14,7 +14,10 @@
     public:
     
         MyMPU6050(PinName i2cSda, PinName i2cScl, PinName interrupt) : mpu(i2cSda, i2cScl), checkpin(interrupt){
-            this->setup(); 
+        }
+        
+        void setPC(Serial *pc){
+            _pc = pc;
         }
         
         void loop() {
@@ -47,7 +50,7 @@
             if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
                 // reset so we can continue cleanly
                 mpu.resetFIFO();
-                //Serial.println(F("FIFO overflow!"));
+                _pc->printf("FIFO overflow!\n");
         
             // otherwise, check for DMP data ready interrupt (this should happen frequently)
             } else if (mpuIntStatus & 0x02) {
@@ -66,14 +69,15 @@
                 mpu.dmpGetGravity(&gravity, &q);
                 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
                 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
-                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];
-                
+                float _theta;
+                if (ypr[1]>0) _theta = ypr[2];
+                else if (ypr[2]<0) _theta = -M_PI-ypr[2];
+                else _theta = M_PI-ypr[2];
+                theta = _theta;
+                                
                 mpu.dmpGetGyro(gyroData, fifoBuffer);
-                dtheta = gyroData[2];
+                float _dtheta = gyroData[2]/180.0*M_PI;
+                dtheta = _dtheta;
             }
         }
         
@@ -88,9 +92,21 @@
         float getDTheta(){
             return dtheta;
         }
+        
+        void disable(){
+            dmpReady = false;            
+            mpuInterrupt = false;
+            checkpin.rise(NULL);
+        }
+        
+        void enable(){
+            setup();
+        }
 
     private:
     
+        Serial *_pc;
+    
         MPU6050 mpu;
         
         bool dmpReady;  // set true if DMP init was successful