sfas
Dependents: Bracky-MPU6050-DMP
Fork of MPU6050 by
Diff: MyMPU6050.h
- Revision:
- 7:07f1f975429a
- Parent:
- 6:748bae310e1e
- Child:
- 8:96fa92188874
diff -r 748bae310e1e -r 07f1f975429a MyMPU6050.h --- 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;