File content as of revision 23:abe76b7c377a:

/*
 * 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_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

/* -- GLOBALS -- */
Ticker pwmint;                              // Button interrupt

BNO055_ID_INF_TypeDef   bno055_id_inf;
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 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

void controlLoop() {
    // Detect the current orientation
    // For now just hardcode it to 'justZ'
    currentConfig = &justZ;
    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;
    }
}

/*
 * TODO: Documentation
 */
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");
    }
    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 
                                // 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);


    
    // 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);

        //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);
    }
}