Diff: main.cpp

Revision:

15:1d21cf90cd47

Parent:

14:90ac96893fcd

Child:

16:27069802baae

--- a/main.cpp	Mon Apr 10 21:00:30 2017 +0000
+++ b/main.cpp	Mon Apr 10 21:11:01 2017 +0000
@@ -23,11 +23,9 @@
     I2C    i2c(PB_9, PB_8);         // SDA, SCL
     BNO055 imu(i2c, PA_8);          // Reset
  
-    BNO055_ID_INF_TypeDef bno055_id_inf;
-    BNO055_EULER_TypeDef  euler_angles;
-    
-    
-    //BNO055_EULER_TypeDef  velocity;  IN PROGRESS
+    BNO055_ID_INF_TypeDef   bno055_id_inf;
+    BNO055_EULER_TypeDef    euler_angles;
+    BNO055_VEL_TypeDef      velocity;  //IN PROGRESS
 
 double Kbt = 5.4;
 double Kbv = 0.33;
@@ -71,7 +69,8 @@
         
     } else {
         pwmint.detach();
-        P1 = 0;
+        P1 = 0.0;
+        r1 = -1.0;
         myled = 0;
         EN1 = 0;
         //P2 = 0;
@@ -104,11 +103,11 @@
     while(1) {
 
         
-        pc.printf("Heading:%+6.4f [rad], R1%+6.4f [PWM] \r\n", //, Pitch:%+6.4f [deg]\r\n",
-                    euler_angles.h, r1, euler_angles.p);
+        pc.printf("Heading:%+6.4f [rad], R1%+6.4f [PWM], Pitch:%+6.4f [rad/s]\r\n",
+                    euler_angles.h, r1, velocity.z);
         
         imu.get_Euler_Angles(&euler_angles);
-
+        imu.get_velocities(&velocity);
         
         //wait(0.2);
     }