Diff: main.cpp

Revision:

16:27069802baae

Parent:

15:1d21cf90cd47

Child:

18:6f120b374991

--- a/main.cpp	Mon Apr 10 21:11:01 2017 +0000
+++ b/main.cpp	Mon Apr 10 21:58:02 2017 +0000
@@ -27,11 +27,11 @@
     BNO055_EULER_TypeDef    euler_angles;
     BNO055_VEL_TypeDef      velocity;  //IN PROGRESS
 
-double Kbt = 5.4;
-double Kbv = 0.33;
-double Kwv = 0.124;
+double Kbt = -89.9276;
+double Kbv = -14.9398;
+double Kwv = -0.0909;
 
-double cur1;
+double wv;
 double r1;
 
 int isPressed;
@@ -39,11 +39,11 @@
 void pwmupdate() {
     
     myled = !myled;
-    cur1 = I1.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
-    cur1 = (cur1-2.0)*5000.0; // Change the value to be in the 0 to 3300 range
+    wv = I1.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
+    wv = (wv-2.0)*5000.0; // Scale the velocity to rad/s
     
     
-    r1 = (Kbt*euler_angles.h);
+    r1 = (Kbt*euler_angles.p + Kbv*velocity.z + Kwv*wv);
     
     r1 = (r1 + 6.0)/12.0 ;              //Normalize for PWM output
     
@@ -103,8 +103,8 @@
     while(1) {
 
         
-        pc.printf("Heading:%+6.4f [rad], R1%+6.4f [PWM], Pitch:%+6.4f [rad/s]\r\n",
-                    euler_angles.h, r1, velocity.z);
+        pc.printf("H:%+6.4f [rad], P:%+6.4f, R:%+6.4f, R1%+6.4f [PWM], Theta_dot:%+6.4f [rad/s] , WV:%+6.4f [rad/s] \r\n",
+                    euler_angles.h, euler_angles.p, euler_angles.r, r1, velocity.z, wv);
         
         imu.get_Euler_Angles(&euler_angles);
         imu.get_velocities(&velocity);