Diff: main.cpp

Revision:

24:c7b3bac429c5

Parent:

23:abe76b7c377a

Child:

25:41abce345a12

diff -r abe76b7c377a -r c7b3bac429c5 main.cpp
--- a/main.cpp	Wed Apr 12 00:29:45 2017 +0000
+++ b/main.cpp	Wed Apr 12 20:54:52 2017 +0000
@@ -13,15 +13,14 @@
 /* Initialize general I/O */
 DigitalOut myled(LED1);
 InterruptIn button(USER_BUTTON);
-Serial pc(SERIAL_TX, SERIAL_RX);            // Serial connection to pc
+Serial *pc;            // Serial connection to pc
 DigitalOut EN1(D0);                         // Interrupt
 I2C    i2c(PB_9, PB_8);                     // SDA, SCL
-BNO055 imu(i2c, PA_8);                      // Reset
+BNO055 *imu;                                // IMU
 
 /* -- GLOBALS -- */
 Ticker pwmint;                              // Button interrupt
 
-BNO055_ID_INF_TypeDef   bno055_id_inf;
 BNO055_EULER_TypeDef    euler_angles;
 BNO055_VEL_TypeDef      velocity;
 
@@ -29,20 +28,67 @@
 bool isActive;                              // Control loop bool
 
 /* Define our parameters here for each balancing configuration */
-struct config justZ = {-89.9276,            //Kbt
+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 = {-89.9276,            //Kbt
+                       -14.9398,            //Kbv
+                       -0.001,              //Kwv
+                       0.8300,              //eqAngle
                        &(euler_angles.p),   //angle
                        &(velocity.z),       //vel
                        new PwmOut(PE_9),    //pwm
-                       new AnalogIn(A0)};   //hall
+                       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;}
+    else {currentConfig = &fall;}
+    
+}
+                       
 void controlLoop() {
     // Detect the current orientation
-    // For now just hardcode it to 'justZ'
-    currentConfig = &justZ;
-    updatePWM(currentConfig);
+    detectOrientation();
+    pc->printf("%s\r\n", currentConfig->descr);
+    
+    // Update the motor torque
+    //updatePWM(currentConfig);
 }
 
 void onButtonPress()
@@ -64,6 +110,7 @@
         //P2 = 0;
         //P3 = 0;
         isActive = false;
+        pc->printf("Push putton to begin loop");
     }
 }
 
@@ -72,40 +119,51 @@
  */
 int main()
 {
-    pc.printf("Cube balance program on " __DATE__ "/" __TIME__ "\r\n");
-    if (imu.chip_ready() == 0) {
-        pc.printf("Bosch BNO055 is NOT available!!\r\n");
+    // Initialize serial
+    pc = new Serial(SERIAL_TX, SERIAL_RX);
+    // Initialize IMU
+    pc->printf("Cube balance program on " __DATE__ "/" __TIME__ "\r\n");
+    wait_ms(1000);                   // Allows IMU time to power up
+    pc->printf("Initializing IMU...\r\n");
+    imu = new BNO055(i2c, PA_8);    // Init IMU to i2c and reset pin
+    while (imu->chip_ready() == 0) {
+        pc->printf("Bosch BNO055 is NOT available!!\r\n");
     }
-    imu.read_id_inf(&bno055_id_inf);
     
     // Initialize pwm to 0 torque
-    justZ.pwm->write(0.5);      //Stops ESCON studio from throwing out-of-range 
+    balZ.pwm->write(0.5);      //Stops ESCON studio from throwing out-of-range 
                                 // error
                                 
     // Set frequency of PWMs
-    justZ.pwm->period(0.0002);  // set pwm frequency
-    
-    //pc.printf("CHIP:0x%02x, ACC:0x%02x, MAG:0x%02x, GYR:0x%02x, , SW:0x%04x, , BL:0x%02x\r\n",
-    //           bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id,
-    //           bno055_id_inf.gyr_id, bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id);
-
-
+    balZ.pwm->period(0.0002);  // set pwm frequency
     
     // Attach the button interrupt to the callback function
     // This button toggles if the loop is enabled
     isActive= false;
     button.rise(&onButtonPress);
+    
+    // Calibrate until sys calib is at 3
+    //checkCalib(imu, pc);
+    
+    // Initialize config
+    detectOrientation();
+    
+    pc->printf("Push button to begin loop");
 
     while(1) {
+        
+        //pc->printf("P:%6.4f, R:%6.4f", 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("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",
+        // 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);
 
-        //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",
+        //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);
+        imu->get_Euler_Angles(&euler_angles);
+        imu->get_velocities(&velocity);
 
         //wait(0.2);
     }