File content as of revision 9:7a8fb72f9a93:

#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 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 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
        );
        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 motor_th(motor_t, NULL, osPriorityNormal, 2048, NULL);

    Thread::wait(osWaitForever);
}