File content as of revision 5:d2e955a94940:

#include "robot.h"
#include "bt_shell.h"



void motor_t(void const *args)
{   
    double angle = 1;
    while(1){
        //move the servo so it reflects the IMU ROLL
        if (((t.read()- prev)*1000) > write_rate){
            angle = imu_data.ypr[1]/90;
            myservo = angle;  
            //angle = angle - 0.01;
            //if (angle < 0)
            //    angle = 1;
            prev = t.read();
        }
        Thread::wait(10);
    }
}

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

void bt_shell(void const *args)
{
    bt_shell_init();
    bt.printf("BT shell initialized\r\n");
    
    while(true) {
        bt_shell_run();
        Thread::wait(10);
    }    
}

int main()
{
    initRobot();

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