File content as of revision 11:425dff6a4af9:

#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 IMU_thread(void const *args)
{
    test_dmp();
    start_dmp(mpu);
    while(true) {
        LED_ON
        update_dmp();
        Thread::yield();
    }
}
void IMU2_thread(void const *args)
{
    test_dmp2();
    start_dmp2(mpu2);
    while(true) {
        LED_OFF
        update_dmp2();
        Thread::yield();
    }
}
void io_thread(void const *args)
{
    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(), getHip(), getKneeDiff()
        );

        Thread::wait(10);
    }
}

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

int main()
{
    t.start();
    
    btSwitch = 1;
    pc.baud(115200);
    
    setup(&sensor);

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

    Thread::wait(osWaitForever);
}