File content as of revision 10:22c44650c7c1:

#include "mbed.h"
#include "rtos.h"
#include "robot.h"
#include "DMP.h"
#include "Sensor.hpp"
#include "control.hpp"
#include "io.h"

Sensor sensor;
DigitalOut btSwitch(PTE30);

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);
    test_dmp2();
    start_dmp2(mpu2);
    while(1) {
        update_dmp();
        update_dmp2();
        Thread::yield();
    }
}

void io_thread(void const *args)
{
    btSwitch = 1;
    pc.baud(115200);
    setup(sensor);

    while(true) {
        if(pc.readable() && pc.getc() == 'c') {
            sensor.calibrate();
            setStateToStop();
        }
        
        process();
        
//        pc.printf(
//            "%.3f;%.3f;%.3f;%i;%.3f;%.3f;%.3f;\r\n", 
//            sensor.time(), sensor.thigh(), sensor.shin(), getState(), getPace(), 0, 0
//        );
        pc.printf("crap\r\n", 0);
        Thread::wait(10);
    }
}

//void control_thread(void const *args)
//{
//    while(true) {
//        process();
//        Thread::wait(10);
//    }
//}

int main()
{
    t.start();

    Thread bt_shell_th(io_thread, NULL, osPriorityNormal, 2048, NULL);
    Thread IMU_th(IMU_thread, NULL, osPriorityNormal, 2048, NULL);
    Thread IMU2_th(IMU2_thread, NULL, osPriorityNormal, 2048, NULL);
//    Thread control_th(control_thread, NULL, osPriorityNormal, 2548, NULL);

    Thread::wait(osWaitForever);
}