Diff: main.cpp

Revision:

26:f2bb916262c9

Parent:

25:41abce345a12

Child:

28:b813a8f685c3

--- a/main.cpp	Thu Apr 13 00:40:51 2017 +0000
+++ b/main.cpp	Thu Apr 13 23:17:41 2017 +0000
@@ -85,7 +85,8 @@
 void controlLoop() {
     // Detect the current orientation
 
-    
+    detectOrientation();
+    //pc->printf("%s\r\n", currentConfig->descr);
     // Update the motor torque
     //updatePWM(currentConfig);
 }
@@ -95,22 +96,23 @@
     wait_ms(10);
     // Activate control loop if not active
     if(!isActive) {
-        pwmint.attach(&controlLoop, .005);
+        pwmint.attach(&controlLoop, .001);
         EN1 = 1;
         myled = 1;
         isActive = true;
+        pc->printf("Loop now active\r\n");
     } 
     
     else {
         pwmint.detach();
-        currentConfig->pwm->write(0.5);
+        //currentConfig->pwm->write(0.5);
         //bt = 0.0;
         myled = 0;
         EN1 = 0;
         //P2 = 0;
         //P3 = 0;
         isActive = false;
-        pc->printf("Push putton to begin loop");
+        pc->printf("Loop now inactive\r\n");
     }
 }
 
@@ -143,10 +145,10 @@
     button.rise(&onButtonPress);
     
     // Calibrate until sys calib is at 3
-    checkCalib(imu, pc);
+    //checkCalib(imu, pc);
     
     // Initialize config
-    detectOrientation();
+    //detectOrientation();
     
     pc->printf("Push button to begin loop");
 
@@ -156,9 +158,9 @@
         //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",
+        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);*/
+            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",
@@ -170,8 +172,7 @@
         imu->get_Euler_Angles(&euler_angles);
         imu->get_velocities(&velocity);
         
-            detectOrientation();
-    pc->printf("%s\r\n", currentConfig->descr);
+
 
         //wait(0.2);
     }