Diff: main.cpp

Revision:

25:41abce345a12

Parent:

24:c7b3bac429c5

Child:

26:f2bb916262c9

--- 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);
     }