File content as of revision 15:1d21cf90cd47:

#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;  //IN PROGRESS

double Kbt = 5.4;
double Kbv = 0.33;
double Kwv = 0.124;

double cur1;
double r1;

int isPressed;

void pwmupdate() {
    
    myled = !myled;
    cur1 = I1.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
    cur1 = (cur1-2.0)*5000.0; // Change the value to be in the 0 to 3300 range
    
    
    r1 = (Kbt*euler_angles.h);
    
    r1 = (r1 + 6.0)/12.0 ;              //Normalize for PWM output
    
    //Limit PWM range
    if (r1 > 1.0){
        r1 = 1.0;
        }
    if (r1 < 0.0){
        r1 = 0.0;
        }
    P1 = r1; 
    //P2 = (euler_angles.r/360.0);
    //P3 = (euler_angles.p/360.0);
    
    }
    
    void eventFunction() {
        
    if(!isPressed) {
        pwmint.attach(&pwmupdate, .005);    
        EN1 = 1;   
        isPressed=1;
        
    } else {
        pwmint.detach();
        P1 = 0.0;
        r1 = -1.0;
        myled = 0;
        EN1 = 0;
        //P2 = 0;
        //P3 = 0;
        isPressed=0;   
    }
}

int main()
{
    
    pc.printf("Bosch Sensortec BNO055 test 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);
    
    //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("Heading:%+6.4f [rad], R1%+6.4f [PWM], Pitch:%+6.4f [rad/s]\r\n",
                    euler_angles.h, r1, velocity.z);
        
        imu.get_Euler_Angles(&euler_angles);
        imu.get_velocities(&velocity);
        
        //wait(0.2);
    }
}