Diff: main.cpp

Revision:

28:b813a8f685c3

Parent:

26:f2bb916262c9

Child:

29:37dc63b57d6e

--- a/main.cpp	Tue Apr 18 03:15:07 2017 +0000
+++ b/main.cpp	Tue Apr 18 19:58:56 2017 +0000
@@ -48,10 +48,10 @@
                        new AnalogIn(A0),    //hall
                        "Balancing on Y edge"};   //descr
 
-struct config balZ = {-89.9276,            //Kbt
-                       -14.9398,            //Kbv
-                       -0.001,              //Kwv
-                       -pi/4,              //eqAngle
+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
@@ -83,12 +83,20 @@
 }
                        
 void controlLoop() {
-    // Detect the current orientation
-
-    detectOrientation();
-    //pc->printf("%s\r\n", currentConfig->descr);
     // Update the motor torque
-    //updatePWM(currentConfig);
+    if(currentConfig->Kbt == 0) {       // detect fallen state
+        pwmint.detach();
+        //currentConfig->pwm->write(0.5);
+        //bt = 0.0;
+        myled = 0;
+        EN1 = 0;
+        //P2 = 0;
+        //P3 = 0;
+        isActive = false;
+        pc->printf("Loop now inactive\r\n");
+    } else {
+        updatePWM(currentConfig);
+    }
 }
 
 void onButtonPress()
@@ -96,7 +104,7 @@
     wait_ms(10);
     // Activate control loop if not active
     if(!isActive) {
-        pwmint.attach(&controlLoop, .001);
+        pwmint.attach(&controlLoop, .01);
         EN1 = 1;
         myled = 1;
         isActive = true;
@@ -116,6 +124,62 @@
     }
 }
 
+void checkCalib(BNO055 *imu, Serial *pc) {
+        pc->printf("Checking calibration status...\r\n");
+    int stat = imu->read_calib_status();
+    while(stat < 192) {
+        pc->printf("Sys:%d Gyr:%d Acc:%d Mag:%d\r\n",
+            (stat >> 6) & 0x03,
+            (stat >> 4) & 0x03,
+            (stat >> 2) & 0x03,
+             stat       & 0x03);
+        wait_ms(250);
+        stat = imu->read_calib_status();
+    }
+    pc->printf("Looks good buddy, put the cube down now.\r\n");
+    wait(2);
+}
+
+/*
+ * Returns PWM duty cycle based on:
+ *  - wv wheel velocity
+ *  - ae angle error
+ *  - bv body velocity
+ *  - Kwv wheel vel gain
+ *  - Kbt angle gain
+ *  - Kbv body vel gain
+ */
+double calcPWM(config *c)
+{
+    // Converts and read the analog input value (value from 0.0 to 1.0):
+    double wv = c->hall->read();
+    wv = (wv - 1.65) * 5000.0 / 1.65; // Scale the velocity to rad/s
+
+    double bt = (*(c->angle) - c->eqAngle);
+
+    double r1 = -(c->Kbt * bt + c->Kbv * -1.0 * (*(c->vel)) + c->Kwv * wv);
+
+    //Limit PWM range
+    if (r1 > 6.0) {r1 = 6.0;}
+    else if (r1 < -6.0) {r1 = -6.0;}
+        
+    // Normalize for PWM output
+    r1 = ((.4*(r1/6.0)) + 0.5);
+    
+    // Check if cube is too far tilted and send 0 torque
+    // May be redundant, check outer program
+    /*if (bt > (pi/8) || bt < -(pi/8)){
+        return .5;
+    }*/
+    return r1;
+}
+
+void updatePWM(config *c) {
+        
+    c->pwm->write(calcPWM(c));
+    //pc->printf("%6.4f\r\n", calcPWM(c));
+}
+
 /*
  * TODO: Documentation
  */
@@ -148,19 +212,24 @@
     //checkCalib(imu, pc);
     
     // Initialize config
-    //detectOrientation();
+    detectOrientation();
     
     pc->printf("Push button to begin loop");
 
     while(1) {
         
+        imu->get_Euler_Angles(&euler_angles);
+        imu->get_velocities(&velocity);
+        detectOrientation();
+        
         //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));
+        //pc->printf("%s\r\n", (currentConfig->descr));
+        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",
@@ -169,11 +238,6 @@
         //pc->printf("Theta:%+6.4f [rad], P:%+6.4f [rad], R1%+6.4f [PWM], Theta_dot:%+6.4f [rad/s], WV:%+6.4f [rad/s] \r\n",
          //         bt, euler_angles.p, r1, velocity.z, wv);
 
-        imu->get_Euler_Angles(&euler_angles);
-        imu->get_velocities(&velocity);
-        
-
-
         //wait(0.2);
     }
 }