File content as of revision 35:34a62f118eb0:

#include "mbed.h"
#include "BNO055.h"


//------------------------------------
// Hyperterminal configuration
// 9600 bauds, 8-bit data, no parity
//------------------------------------

Serial pc(SERIAL_TX, SERIAL_RX);

Ticker pwmint;
DigitalOut myled(LED1);
InterruptIn button(USER_BUTTON);

PwmOut P1(PE_9);
DigitalOut EN1(D0);
AnalogIn I1(A0);

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

BNO055_ID_INF_TypeDef   bno055_id_inf;
BNO055_EULER_TypeDef    euler_angles;
BNO055_VEL_TypeDef      velocity;  
            //Pole P + f//Pole P    //LQR 2000  //LQR 500   //LQR 1000  
double Kbt = -67.6352;   //-67.5155;  //-73.8411; //-83.4030; //-77.7950; 
double Kbv = -9.3204;   //-9.3610;   //-9.3563;  //-10.5874; //-9.8654;  
double Kwv = -0.0094;//82;//587;   //-0.0080;   //-0.00244; //-0.00468; //-0.00337; 
double speed;
double wv;
double bt;
double bv;
double r1;

double pi = 3.14159265;

int isPressed;

void pwmupdate()
{

   
    speed = I1.read();              // Converts and read the analog input value (value from 0.0 to 1.0)
    wv = (speed-0.5)*(5000/0.5);    // Scale the velocity to rad/s

    bt = ((euler_angles.p) + (pi/4));       //Read body angle
    bv = -1.0*velocity.z;                   //Read boady angle velocity
    
    r1 = -1.0*(Kbt*bt + Kbv*bv + Kwv*wv);   //Calculate current setpoint

    
    //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;                                    //
    
    
    //Limit angle to prevent output railing
    
    if (bt > (pi/6)){
        EN1 = 0;
        pwmint.detach();
        }
        
    else if (bt< -(pi/6)){
        EN1 = 0; 
        pwmint.detach();
        }
        
}

void eventFunction()
{
    //Button press routine
    
    
    if(!isPressed) {
        //Enable closed loop mode, enable motor drivers
        pwmint.attach(&pwmupdate, .002);
        EN1 = 1;
        myled = 1;
        isPressed=1;

    } 
    
    else {
        //Disable closed loop mode
        pwmint.detach();
        P1 = 0.5;
        bt = 0.0;
        myled = 0;
        EN1 = 0;

        isPressed=0;
    }
}

int main()
{
    wait_us(2000000);
    
    pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n");
    pc.printf("Kbt: %+6.4f, Kbv: %+6.4f, Kwv: %+6.4f\r\n", Kbt,Kbv,Kwv);
    
    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
    
    
    //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



    while(1) {


        // pc.printf("H:%+6.4f [rad], P:%+6.4f, R:%+6.4f, THETA%+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, bt, 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, bv, wv);

        imu.get_Euler_Angles(&euler_angles);
        imu.get_velocities(&velocity);
        
        //bt = ((euler_angles.p) + (pi/4));
        //bv = -1.0*velocity.z;
        //wait(0.2);
    }
}