Diff: main.cpp

Revision:

29:37dc63b57d6e

Parent:

28:b813a8f685c3

--- a/main.cpp	Tue Apr 18 19:58:56 2017 +0000
+++ b/main.cpp	Tue Apr 18 21:13:06 2017 +0000
@@ -18,66 +18,15 @@
 I2C    i2c(PB_9, PB_8);                     // SDA, SCL
 BNO055 *imu;                                // IMU
 
-/* -- GLOBALS -- */
-Ticker pwmint;                              // Button interrupt
-
-BNO055_EULER_TypeDef    euler_angles;
-BNO055_VEL_TypeDef      velocity;
-
-config *currentConfig;                      // Stores current config
-bool isActive;                              // Control loop bool
-
-/* Define our parameters here for each balancing configuration */
-struct config balX = {-89.9276,            //Kbt
-                       -14.9398,            //Kbv
-                       -0.001,              //Kwv
-                       pi/4.0,              //eqAngle
-                       &(euler_angles.r),   //angle
-                       &(velocity.x),       //vel
-                       new PwmOut(PE_9),    //pwm
-                       new AnalogIn(A0),    //hall
-                       "Balancing on X edge"};   //descr
-                       
-struct config balY = {0,            //Kbt
-                       0,            //Kbv
-                       0,              //Kwv
-                       -pi/4.0,              //eqAngle
-                       &(euler_angles.p),   //angle
-                       &(velocity.y),       //vel
-                       new PwmOut(PE_9),    //pwm
-                       new AnalogIn(A0),    //hall
-                       "Balancing on Y edge"};   //descr
-
-struct config balZ = {-72.4921,            //Kbt
-                       -9.9672,            //Kbv
-                       -0.00025,              //Kwv
-                       -.7000,              //eqAngle
-                       &(euler_angles.p),   //angle
-                       &(velocity.z),       //vel
-                       new PwmOut(PE_9),    //pwm
-                       new AnalogIn(A0),    //hall
-                       "Balancing on Z edge"};   //descr
-
-struct config fall = {0,            //Kbt
-                       0,            //Kbv
-                       0,              //Kwv
-                       0,              //eqAngle
-                       &(euler_angles.p),   //angle
-                       &(velocity.z),       //vel
-                       NULL,    //pwm
-                       NULL,    //hall
-                       "Fallen"};   //descr
                  
 void detectOrientation() {
-    float tm = .25;  //threshold for main axis
-    float ta = .2;   //threshold for aux axis
     
-    if (abs(euler_angles.r - balX.eqAngle) < tm
-        && abs(euler_angles.p) < ta) {currentConfig = &balX;}
-    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;}
+    if (abs(euler_angles.r - balX.eqAngle) < TM
+        && abs(euler_angles.p) < TA) {currentConfig = &balX;}
+    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;}
     else {currentConfig = &fall;}
     
 }
@@ -101,9 +50,12 @@
 
 void onButtonPress()
 {
-    wait_ms(10);
+    wait_ms(100); // Debounce button
+    
     // Activate control loop if not active
     if(!isActive) {
+        // Reset equilibrium angle to current angle
+        currentConfig->eqAngle = *(currentConfig->angle);
         pwmint.attach(&controlLoop, .01);
         EN1 = 1;
         myled = 1;