File content as of revision 6:b3baf0fe5b73:

#include "mbed.h"
#include "rtos.h"
#include "robot.h"
#include "DMP.h"

Serial pc(PTA2,PTA1);
DigitalOut btSwitch(PTE30);

void print_all()    //call this function to print all sensor data
{
    float data[13];
    data[0]= getTime();
    data[1]= imu_data.ypr[0];
    data[2]= imu_data.ypr[1];
    data[3]= imu_data.ypr[2];

    data[4]= imu2_data.ypr[0];
    data[5]= imu2_data.ypr[1];
    data[6]= imu2_data.ypr[2];

    pc.printf("&;");
    pc.printf("%f;",data[0]);       //print time as an integer (milliseconds)
    for(int i = 1; i<=6; i++) {
        pc.printf("%.3f;",data[i]);
    }
    pc.printf("\n\r");
    //commented out the below code because we want to ignore the rear IR sensors
    /*dt = t.read() - data[0];
    irBack = 1;
    if(dt < 50)
        Thread::wait(50-dt);

    data[6]= irR.read();
    data[7]= irL.read();
    data[8]= imu_data.ypr[0]*180/PI;
    irBack = 0;
    */

}

void motor_t(void const *args)
{
//    myservo = 0;
}

void IMU2_thread(void const *args)
{
    test_dmp2();
    start_dmp2(mpu2);
    while(1) {
        update_dmp2();
        Thread::yield();
    }
}
void IMU_thread(void const *args)
{
    test_dmp();
    start_dmp(mpu);

    while(1) {
        update_dmp();
        Thread::yield();
    }
}

void bt_shell(void const *args)
{
    btSwitch = 1;
    pc.baud(115200);

    int command = 0;
    while(true) {
//        if(pc.readable()) {
//            pc.scanf("%d;", &command);
//        }
//
//        pc.printf("%.3f;%d\r\n", getTime(), command);
        print_all();
        Thread::wait(50);
    }
}

int main()
{
    t.start();

    Thread bt_shell_th(bt_shell, NULL, osPriorityNormal, 2048, NULL);
    Thread IMU_th(IMU_thread, NULL, osPriorityNormal, 2048, NULL);
    Thread IMU2_th(IMU2_thread, NULL, osPriorityNormal, 2048, NULL);
    Thread motor_th(motor_t, NULL, osPriorityNormal, 2048, NULL);

    Thread::wait(osWaitForever);
}