main.cpp

00001 /*
00002  * main.cpp
00003  * April 11, 2017
00004  *
00005  * Control software for balancing cube senior design project
00006  *
00007  * Will Church
00008  * Tom Rasmussen
00009  */
00010 
00011 #include "cube.h"
00012 
00013 /* Initialize general I/O */
00014 DigitalOut myled(LED1);
00015 InterruptIn button(USER_BUTTON);
00016 Serial *pc;            // Serial connection to pc
00017 DigitalOut EN1(D0);                         // Interrupt
00018 I2C    i2c(PB_9, PB_8);                     // SDA, SCL
00019 BNO055 *imu;                                // IMU
00020 
00021                  
00022 void detectOrientation() {
00023     
00024     if (abs(euler_angles.r - balX.eqAngle) < TM
00025         && abs(euler_angles.p) < TA) {currentConfig = &balX;}
00026     else if (abs(euler_angles.p - balY.eqAngle) < TM
00027         && abs(euler_angles.r) < TA) {currentConfig = &balY;}
00028     else if (abs(euler_angles.p - balZ.eqAngle) < TM
00029         && abs(euler_angles.r + 1.6) < TA) {currentConfig = &balZ;}
00030     else {currentConfig = &fall;}
00031     
00032 }
00033                        
00034 void controlLoop() {
00035     // Update the motor torque
00036     if(currentConfig->Kbt == 0) {       // detect fallen state
00037         pwmint.detach();
00038         //currentConfig->pwm->write(0.5);
00039         //bt = 0.0;
00040         myled = 0;
00041         EN1 = 0;
00042         //P2 = 0;
00043         //P3 = 0;
00044         isActive = false;
00045         pc->printf("Loop now inactive\r\n");
00046     } else {
00047         updatePWM(currentConfig);
00048     }
00049 }
00050 
00051 void onButtonPress()
00052 {
00053     wait_ms(100); // Debounce button
00054     
00055     // Activate control loop if not active
00056     if(!isActive) {
00057         // Reset equilibrium angle to current angle
00058         currentConfig->eqAngle = *(currentConfig->angle);
00059         pwmint.attach(&controlLoop, .01);
00060         EN1 = 1;
00061         myled = 1;
00062         isActive = true;
00063         pc->printf("Loop now active\r\n");
00064     } 
00065     
00066     else {
00067         pwmint.detach();
00068         //currentConfig->pwm->write(0.5);
00069         //bt = 0.0;
00070         myled = 0;
00071         EN1 = 0;
00072         //P2 = 0;
00073         //P3 = 0;
00074         isActive = false;
00075         pc->printf("Loop now inactive\r\n");
00076     }
00077 }
00078 
00079 void checkCalib(BNO055 *imu, Serial *pc) {
00080         pc->printf("Checking calibration status...\r\n");
00081     int stat = imu->read_calib_status();
00082     while(stat < 192) {
00083         pc->printf("Sys:%d Gyr:%d Acc:%d Mag:%d\r\n",
00084             (stat >> 6) & 0x03,
00085             (stat >> 4) & 0x03,
00086             (stat >> 2) & 0x03,
00087              stat       & 0x03);
00088         wait_ms(250);
00089         stat = imu->read_calib_status();
00090     }
00091     pc->printf("Looks good buddy, put the cube down now.\r\n");
00092     wait(2);
00093 }
00094 
00095 /*
00096  * Returns PWM duty cycle based on:
00097  *  - wv wheel velocity
00098  *  - ae angle error
00099  *  - bv body velocity
00100  *  - Kwv wheel vel gain
00101  *  - Kbt angle gain
00102  *  - Kbv body vel gain
00103  */
00104 double calcPWM(config *c)
00105 {
00106     // Converts and read the analog input value (value from 0.0 to 1.0):
00107     double wv = c->hall->read();
00108     wv = (wv - 1.65) * 5000.0 / 1.65; // Scale the velocity to rad/s
00109 
00110     double bt = (*(c->angle) - c->eqAngle);
00111 
00112     double r1 = -(c->Kbt * bt + c->Kbv * -1.0 * (*(c->vel)) + c->Kwv * wv);
00113 
00114     //Limit PWM range
00115     if (r1 > 6.0) {r1 = 6.0;}
00116     else if (r1 < -6.0) {r1 = -6.0;}
00117         
00118     // Normalize for PWM output
00119     r1 = ((.4*(r1/6.0)) + 0.5);
00120     
00121     // Check if cube is too far tilted and send 0 torque
00122     // May be redundant, check outer program
00123     /*if (bt > (pi/8) || bt < -(pi/8)){
00124         return .5;
00125     }*/
00126     return r1;
00127 }
00128 
00129 void updatePWM(config *c) {
00130         
00131     c->pwm->write(calcPWM(c));
00132     //pc->printf("%6.4f\r\n", calcPWM(c));
00133 }
00134 
00135 /*
00136  * TODO: Documentation
00137  */
00138 int main()
00139 {
00140     // Initialize serial
00141     pc = new Serial(SERIAL_TX, SERIAL_RX);
00142     // Initialize IMU
00143     pc->printf("Cube balance program on " __DATE__ "/" __TIME__ "\r\n");
00144     wait_ms(1000);                   // Allows IMU time to power up
00145     pc->printf("Initializing IMU...\r\n");
00146     imu = new BNO055(i2c, PA_8);    // Init IMU to i2c and reset pin
00147     while (imu->chip_ready() == 0) {
00148         pc->printf("Bosch BNO055 is NOT available!!\r\n");
00149     }
00150     
00151     // Initialize pwm to 0 torque
00152     balZ.pwm->write(0.5);      //Stops ESCON studio from throwing out-of-range 
00153                                 // error
00154                                 
00155     // Set frequency of PWMs
00156     balZ.pwm->period(0.0002);  // set pwm frequency
00157     
00158     // Attach the button interrupt to the callback function
00159     // This button toggles if the loop is enabled
00160     isActive= false;
00161     button.rise(&onButtonPress);
00162     
00163     // Calibrate until sys calib is at 3
00164     //checkCalib(imu, pc);
00165     
00166     // Initialize config
00167     detectOrientation();
00168     
00169     pc->printf("Push button to begin loop");
00170 
00171     while(1) {
00172         
00173         imu->get_Euler_Angles(&euler_angles);
00174         imu->get_velocities(&velocity);
00175         detectOrientation();
00176         
00177         //pc->printf("P:%6.4f, R:%6.4f\r\n", euler_angles.p, euler_angles.r);
00178         //pc->printf("X:%6.4f, Y:%6.4f, Z:%6.4f\r\n", velocity.x, velocity.y, velocity.z);
00179         //pc->printf("%s\r\n", (currentConfig->descr));
00180         pc->printf("%6.4f\r\n", *(currentConfig->angle));
00181         // 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);
00182         /*pc->printf("%6.4f %6.4f %6.4f %6.4f %6.4f %6.4f\r\n",
00183             euler_angles.h * 180 / pi, euler_angles.p * 180 / pi, euler_angles.r * 180 / pi, 
00184             euler_angles.h * 180 / pi, euler_angles.p * 180 / pi, euler_angles.r * 180 / pi);*/
00185 
00186 
00187         // 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",
00188         //             euler_angles.h, euler_angles.p, euler_angles.r, r1, velocity.z, wv);
00189 
00190         //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",
00191          //         bt, euler_angles.p, r1, velocity.z, wv);
00192 
00193         //wait(0.2);
00194     }
00195 }