File content as of revision 5:46947b447701:

#include "Controller.h"

InterruptIn   button(p23);
InterruptIn   imu(p16);
Serial pc(USBTX, USBRX);

Controller::Controller()
{
    this->modeList.push_back(new TrainingCommand());
    this->modeList.push_back(new OfflineCommand());
    this->modeList.push_back(new ReadCommand());
    this->modeList.push_back(new IdleCommand());
    this->modeList.push_back(new SleepCommand());

    button.fall(this, &Controller::buttonInt);
    imu.fall(this, &Controller::imuInt);

    //Set idle command at startup
    this->currentMode = 3;

    while(Sensors::getInstance()->getFs() != 100) {
        Sensors::getInstance()->setupIMU();
        wait(0.01);
        Sensors::getInstance()->disableIMU();
    }

    this->modeList[currentMode]->initialize();
}

void Controller::setCommand(int n)
{
    this->modeList[currentMode]->finish();
    this->currentMode = n;
    this->modeList[currentMode]->initialize();
}

void Controller::imuInt()
{
    this->modeList[currentMode]->execute();
}

void Controller::buttonInt()
{
    wait(0.25);
    this->modeList[currentMode]->button();
}

void Controller::run()
{
    Sensors::getInstance()->setupIMU();
    Sensors::getInstance()->disableIMU();

    pc.baud(19200);

    while(1) {

        if(pc.readable()) {

            char command = pc.getc();

            switch(command) {
                case '1':
                    pc.printf("een\n");
                    setCommand(0);

                    break;
                case '2':
                    pc.printf("twee\n");
                    setCommand(1);
                    break;
                case '3':
                    pc.printf("drie\n");
                    setCommand(2);
                    break;
                case '4':
                    pc.printf("vier\n");
                    setCommand(3);
                    break;
                case '5':
                    pc.printf("vijf\n");
                    setCommand(4);
                    break;
            }
        }
    }


}