File content as of revision 8:c6345e8d607c:

#include "Controller.h"
#include "InterruptManager.h"

InterruptIn   imu(p16);
InterruptIn   button(p23);


//Ticker ticker;
//Ticker tickerData;

Controller::Controller()
{
    buttonIntFlag = false;
    imuIntFlag = false;
    tickerIntFlag = false;
    currentModeSelector = 3;
    tickerIntDataFlag = false;

    angle = 0;
    peak = false;

    sensors = new Sensors();
    storage = new Storage();
    gonioService = new GonioService();

    //Set up IMU with sample frequency
    sensors->setupIMU(0x4f);

    modeList.push_back(new TrainingCommand(sensors, gonioService));
    modeList.push_back(new OfflineCommand(sensors, storage));
    modeList.push_back(new ReadCommand(storage, gonioService));
    modeList.push_back(new IdleCommand(sensors, gonioService));
    modeList.push_back(new SleepCommand(sensors));

    wait(0.01);

    //sensors->disableIMU();

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

    //Set idle command at startup
    currentMode()->initialize();
}

void Controller::setCommand(int n)
{
    currentMode()->finish();
    currentModeSelector = n;
    currentMode()->initialize();
}

Command* Controller::currentMode()
{
    return modeList[currentModeSelector];
}

void Controller::imuInt()
{
     
     imuIntFlag = true;
}

void Controller::buttonInt()
{
    wait(0.25); //wait for debounce
    buttonIntFlag = true;
}

void Controller::tickerInt()
{
    tickerIntFlag = true;
}

void Controller::tickerDataInt()
{
    tickerIntDataFlag = true;
}

void Controller::run()
{
    /*
    ticker.attach(this, &Controller::tickerInt, 0.1);
    wait(0.05);
    tickerData.attach(this, &Controller::tickerDataInt, 0.1);
    */


    while(1) {
        
        if(gonioService->isConnected() & gonioService->newValue()){
            
            int8_t command = gonioService->getWriteValue() - 1;
            printf("%d", command);
            switch(command) {
                case 0 :
                    printf("een\n");
                    setCommand(0);
                    break;
                case 1 :
                    printf("twee\n");
                    gonioService->disconnect();
                    setCommand(1);
                    break;
                case 2 :
                    printf("drie\n");
                    setCommand(2);
                    break;
                case 3 :
                    printf("vier\n");
                    setCommand(3);
                    break;
                }
            }
        
        
        if(buttonIntFlag) {
            buttonIntFlag = false;
            currentMode()->button();
        }

        if(tickerIntDataFlag) {
            tickerIntDataFlag = false;
        }

        if(tickerIntFlag) {
            tickerIntFlag = false;
        }

        if(imuIntFlag) {
            imuIntFlag = false;
            currentMode()->execute();
        }
    }
}