File content as of revision 6:75263c93daf7:

#include "Controller.h"

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

Controller::Controller()
{
    this->sensors = new Sensors();
    this->storage = new Storage();
    
    this->modeList.push_back(new TrainingCommand(sensors));
    this->modeList.push_back(new OfflineCommand(sensors, storage));
    this->modeList.push_back(new ReadCommand(storage));
    this->modeList.push_back(new IdleCommand(sensors));
    this->modeList.push_back(new SleepCommand(sensors));

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

    //Set idle command at startup
    this->currentMode = 3;
    
    sensors->setupIMU();
    sensors->disableIMU();
    wait(0.1);
    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()
{
    pc.baud(19200);

    while(1) {

        if(pc.readable()) {

            char command = pc.getc();

            switch(command) {
                case '1':
                    setCommand(0);
                    break;
                case '2':
                    setCommand(1);
                    break;
                case '3':
                    setCommand(2);
                    break;
                case '4':
                    setCommand(3);
                    break;
                case '5':
                    setCommand(4);
                    break;
            }
        }
    }


}