![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Dependencies: BNO055_fusion_tom FastPWM mbed
Fork of NucleoCube1 by
Diff: main.cpp
- Revision:
- 16:27069802baae
- Parent:
- 15:1d21cf90cd47
- Child:
- 18:6f120b374991
diff -r 1d21cf90cd47 -r 27069802baae main.cpp --- 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);