File content as of revision 24:c7b3bac429c5:

/*
 * main.cpp
 * April 11, 2017
 *
 * Control software for balancing cube senior design project
 *
 * Will Church
 * Tom Rasmussen
 */

#include "cube.h"

/* Initialize general I/O */
DigitalOut myled(LED1);
InterruptIn button(USER_BUTTON);
Serial *pc;            // Serial connection to pc
DigitalOut EN1(D0);                         // Interrupt
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 = {-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
                       "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
    detectOrientation();
    pc->printf("%s\r\n", currentConfig->descr);
    
    // Update the motor torque
    //updatePWM(currentConfig);
}

void onButtonPress()
{
    // Activate control loop if not active
    if(!isActive) {
        pwmint.attach(&controlLoop, .005);
        EN1 = 1;
        myled = 1;
        isActive = true;
    } 
    
    else {
        pwmint.detach();
        currentConfig->pwm->write(0.5);
        //bt = 0.0;
        myled = 0;
        EN1 = 0;
        //P2 = 0;
        //P3 = 0;
        isActive = false;
        pc->printf("Push putton to begin loop");
    }
}

/*
 * TODO: Documentation
 */
int main()
{
    // 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");
    }
    
    // Initialize pwm to 0 torque
    balZ.pwm->write(0.5);      //Stops ESCON studio from throwing out-of-range 
                                // error
                                
    // Set frequency of PWMs
    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",
        //             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",
         //         bt, euler_angles.p, r1, velocity.z, wv);

        imu->get_Euler_Angles(&euler_angles);
        imu->get_velocities(&velocity);

        //wait(0.2);
    }
}