Dependencies:   BNO055_fusion_tom FastPWM mbed

Fork of NucleoCube1 by Will Church

main.cpp

Committer:
wchurch
Date:
2017-04-03
Revision:
1:d81f93d3f819
Parent:
0:604ceafb7bb3
Child:
2:93c7a7fb3203

File content as of revision 1:d81f93d3f819:

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


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

Serial pc(SERIAL_TX, SERIAL_RX);

DigitalOut myled(LED1);
PwmOut P1(PE_9);
PwmOut P2(PE_11);
PwmOut P3(PE_13);

int main()
{
    //int i = 1;
    pc.printf("Hello World !\n");
    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;
    
    pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n");
    if (imu.chip_ready() == 0){
        pc.printf("Bosch BNO055 is NOT avirable!!\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);
    
    while(1) {
//        acc.getAccAllAxis(data); 
//        pc.printf("Data %d %d %d \n",data[0], data[1], data[2]);
        imu.get_Euler_Angles(&euler_angles);
        pc.printf("Heading:%+6.4f [deg], Roll:%+6.4f [deg], Pitch:%+6.4f [deg]\r\n",
                    euler_angles.h, euler_angles.r, euler_angles.p);
        wait(0.2);
        
        //pc.printf("This program runs since %d seconds.\n", i++);
        //pc.putc(pc.getc());
        myled = !myled;
        P1 = .25;//(euler_angles.h/360.0);
        P2 = .5; //(euler_angles.r/360.0);
        P3 = .75; //(euler_angles.p/360.0);
    }
}