cube
Dependencies: BNO055_fusion_tom FastPWM mbed
Diff: main.cpp
- Revision:
- 36:54b527b3a576
- Parent:
- 35:34a62f118eb0
- Child:
- 37:55cae2b7033e
diff -r 34a62f118eb0 -r 54b527b3a576 main.cpp --- 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; }