![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Dependencies: BNO055_fusion_tom FastPWM mbed
Fork of NucleoCube1 by
Diff: main.cpp
- Revision:
- 25:41abce345a12
- Parent:
- 24:c7b3bac429c5
- Child:
- 26:f2bb916262c9
diff -r c7b3bac429c5 -r 41abce345a12 main.cpp --- a/main.cpp Wed Apr 12 20:54:52 2017 +0000 +++ b/main.cpp Thu Apr 13 00:40:51 2017 +0000 @@ -41,7 +41,7 @@ struct config balY = {0, //Kbt 0, //Kbv 0, //Kwv - pi/4.0, //eqAngle + -pi/4.0, //eqAngle &(euler_angles.p), //angle &(velocity.y), //vel new PwmOut(PE_9), //pwm @@ -51,7 +51,7 @@ struct config balZ = {-89.9276, //Kbt -14.9398, //Kbv -0.001, //Kwv - 0.8300, //eqAngle + -pi/4, //eqAngle &(euler_angles.p), //angle &(velocity.z), //vel new PwmOut(PE_9), //pwm @@ -77,15 +77,14 @@ else if (abs(euler_angles.p - balY.eqAngle) < tm && abs(euler_angles.r) < ta) {currentConfig = &balY;} else if (abs(euler_angles.p - balZ.eqAngle) < tm - && abs(euler_angles.r - 1.6) < ta) {currentConfig = &balZ;} + && abs(euler_angles.r + 1.6) < ta) {currentConfig = &balZ;} else {currentConfig = &fall;} } void controlLoop() { // Detect the current orientation - detectOrientation(); - pc->printf("%s\r\n", currentConfig->descr); + // Update the motor torque //updatePWM(currentConfig); @@ -93,6 +92,7 @@ void onButtonPress() { + wait_ms(10); // Activate control loop if not active if(!isActive) { pwmint.attach(&controlLoop, .005); @@ -143,7 +143,7 @@ button.rise(&onButtonPress); // Calibrate until sys calib is at 3 - //checkCalib(imu, pc); + checkCalib(imu, pc); // Initialize config detectOrientation(); @@ -152,9 +152,14 @@ while(1) { - //pc->printf("P:%6.4f, R:%6.4f", euler_angles.p, euler_angles.r); + //pc->printf("P:%6.4f, R:%6.4f\r\n", euler_angles.p, euler_angles.r); //pc->printf("X:%6.4f, Y:%6.4f, Z:%6.4f\r\n", velocity.x, velocity.y, velocity.z); //pc->printf("%6.4f\r\n", *(currentConfig->angle)); + // cuberotate: pc->printf("Orientation: %6.4f %6.4f %6.4f\r\n", euler_angles.h * 180 / pi, euler_angles.p * 180 / pi, euler_angles.r * 180 / pi); + /*pc->printf("%6.4f %6.4f %6.4f %6.4f %6.4f %6.4f\r\n", + euler_angles.h * 180 / pi, euler_angles.p * 180 / pi, euler_angles.r * 180 / pi, + euler_angles.h * 180 / pi, euler_angles.p * 180 / pi, euler_angles.r * 180 / pi);*/ + // 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); @@ -164,6 +169,9 @@ imu->get_Euler_Angles(&euler_angles); imu->get_velocities(&velocity); + + detectOrientation(); + pc->printf("%s\r\n", currentConfig->descr); //wait(0.2); }