Diff: main.cpp

Revision:

36:54b527b3a576

Parent:

35:34a62f118eb0

Child:

37:55cae2b7033e

--- a/main.cpp	Sat Apr 22 21:09:29 2017 +0000
+++ b/main.cpp	Tue Apr 25 19:11:41 2017 +0000
@@ -26,10 +26,13 @@
 BNO055_ID_INF_TypeDef   bno055_id_inf;
 BNO055_EULER_TypeDef    euler_angles;
 BNO055_VEL_TypeDef      velocity;  
-            //Pole P + f//Pole P    //LQR 2000  //LQR 500   //LQR 1000  
-double Kbt = -67.6352;   //-67.5155;  //-73.8411; //-83.4030; //-77.7950; 
-double Kbv = -9.3204;   //-9.3610;   //-9.3563;  //-10.5874; //-9.8654;  
-double Kwv = -0.0094;//82;//587;   //-0.0080;   //-0.00244; //-0.00468; //-0.00337; 
+
+
+            //Pole P + f    //Pole P     //LQR 2000  //LQR 500   //LQR 1000  
+double Kbt = -67.6352;      //-67.5155;  //-73.8411; //-83.4030; //-77.7950; 
+double Kbv = -9.3204;       //-9.3610;   //-9.3563;  //-10.5874; //-9.8654;  
+double Kwv = -0.0082;       //82;//587;  //-0.0080;  //-0.00244; //-0.00468; //-0.00337; 
+
 double speed;
 double wv;
 double bt;
@@ -50,6 +53,8 @@
     bt = ((euler_angles.p) + (pi/4));       //Read body angle
     bv = -1.0*velocity.z;                   //Read boady angle velocity
     
+    
+    //THIS IS THE ONE LINE OF CODE THAT MATTERS
     r1 = -1.0*(Kbt*bt + Kbv*bv + Kwv*wv);   //Calculate current setpoint
 
     
@@ -58,7 +63,7 @@
         r1 = 6.0;
     }
     
-    if (r1 < -6.0) {
+    else if (r1 < -6.0) {
         r1 = -6.0;
     }