Diff: main.cpp

Revision:

23:abe76b7c377a

Parent:

22:9f3022fe9084

Child:

24:c7b3bac429c5

diff -r 9f3022fe9084 -r abe76b7c377a main.cpp
--- a/main.cpp	Tue Apr 11 17:48:48 2017 +0000
+++ b/main.cpp	Wed Apr 12 00:29:45 2017 +0000
@@ -1,130 +1,103 @@
-#include "mbed.h"
-#include "BNO055.h"
-
+/*
+ * main.cpp
+ * April 11, 2017
+ *
+ * Control software for balancing cube senior design project
+ *
+ * Will Church
+ * Tom Rasmussen
+ */
 
-//------------------------------------
-// Hyperterminal configuration
-// 9600 bauds, 8-bit data, no parity
-//------------------------------------
+#include "cube.h"
 
-Serial pc(SERIAL_TX, SERIAL_RX);
-
-Ticker pwmint;
+/* Initialize general I/O */
 DigitalOut myled(LED1);
 InterruptIn button(USER_BUTTON);
-
-PwmOut P1(PE_9);
-DigitalOut EN1(D0);
-AnalogIn I1(A0);
+Serial pc(SERIAL_TX, SERIAL_RX);            // Serial connection to pc
+DigitalOut EN1(D0);                         // Interrupt
+I2C    i2c(PB_9, PB_8);                     // SDA, SCL
+BNO055 imu(i2c, PA_8);                      // Reset
 
-//PwmOut P2(PE_11);     1D FOCUS FOR NOW
-//PwmOut P3(PE_13);
-
-I2C    i2c(PB_9, PB_8);         // SDA, SCL
-BNO055 imu(i2c, PA_8);          // Reset
+/* -- GLOBALS -- */
+Ticker pwmint;                              // Button interrupt
 
 BNO055_ID_INF_TypeDef   bno055_id_inf;
 BNO055_EULER_TypeDef    euler_angles;
-BNO055_VEL_TypeDef      velocity;  //IN PROGRESS
-
-double Kbt = -89.9276;
-double Kbv = -14.9398;
-double Kwv =  -0.001; //-0.0909; 
-double wv;
-double bt;
-double r1;
-
-double pi = 3.14159265;
+BNO055_VEL_TypeDef      velocity;
 
-int isPressed;
-
-void pwmupdate()
-{
-
-    //myled = !myled;
-    wv = I1.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
-    wv = (wv-2.0)*5000.0; // Scale the velocity to rad/s
-
-    bt = (euler_angles.p - (pi/4.0));
-
-    r1 = (Kbt*bt + Kbv*velocity.z + Kwv*wv);
+config *currentConfig;                      // Stores current config
+bool isActive;                              // Control loop bool
 
-    
+/* Define our parameters here for each balancing configuration */
+struct config justZ = {-89.9276,            //Kbt
+                       -14.9398,            //Kbv
+                       -0.001,              //Kwv
+                       pi/4.0,              //eqAngle
+                       &(euler_angles.p),   //angle
+                       &(velocity.z),       //vel
+                       new PwmOut(PE_9),    //pwm
+                       new AnalogIn(A0)};   //hall
 
-    //Limit PWM range
-    if (r1 > 6.0) {
-        r1 = 6.0;
-    }
-    
-    if (r1 < -6.0) {
-        r1 = -6.0;
-    }
-        
-    r1 = ((.4*(r1/6.0)) + 0.5) ;              //Normalize for PWM output
-    
-    P1 = r1;
-    //P2 = (euler_angles.r/360.0);
-    //P3 = (euler_angles.p/360.0);
-    if (bt > (pi/8)){
-        EN1 = 0;
-        pwmint.detach();
-        }
-        
-    else if (bt< -(pi/8)){
-        EN1 = 0; 
-        pwmint.detach();
-        }
-        
+void controlLoop() {
+    // Detect the current orientation
+    // For now just hardcode it to 'justZ'
+    currentConfig = &justZ;
+    updatePWM(currentConfig);
 }
 
-void eventFunction()
+void onButtonPress()
 {
-
-    if(!isPressed) {
-        pwmint.attach(&pwmupdate, .005);
+    // Activate control loop if not active
+    if(!isActive) {
+        pwmint.attach(&controlLoop, .005);
         EN1 = 1;
         myled = 1;
-        isPressed=1;
-
+        isActive = true;
     } 
     
     else {
         pwmint.detach();
-        P1 = 0.5;
-        bt = 0.0;
+        currentConfig->pwm->write(0.5);
+        //bt = 0.0;
         myled = 0;
         EN1 = 0;
         //P2 = 0;
         //P3 = 0;
-        isPressed=0;
+        isActive = false;
     }
 }
 
+/*
+ * TODO: Documentation
+ */
 int main()
 {
-
-    pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n");
+    pc.printf("Cube balance program on " __DATE__ "/" __TIME__ "\r\n");
     if (imu.chip_ready() == 0) {
         pc.printf("Bosch BNO055 is NOT available!!\r\n");
     }
-
     imu.read_id_inf(&bno055_id_inf);
-    P1 = .5;        //Stops ESCON studio from throwing out-of-range error
+    
+    // Initialize pwm to 0 torque
+    justZ.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);
 
-    P1.period(0.0002);      //Set PWM frequency
 
-    isPressed=0;
-    button.rise(&eventFunction);        //Enable Closed Loop
-
-
+    
+    // Attach the button interrupt to the callback function
+    // This button toggles if the loop is enabled
+    isActive= false;
+    button.rise(&onButtonPress);
 
     while(1) {
 
-
         // 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);