File content as of revision 4:2a5a08b14539:

#include "Controller.h"

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

Controller::Controller(){
    this->modeList.push_back(new TrainingCommand());
    this->modeList.push_back(new CompetitionCommand());
    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);
    
    this->currentMode = 3;
    this->modeList[currentMode]->initialize();
    }

//FOR GONIOTRINER CLASS
Command* Controller::getController(int i){
    return this->modeList[i];
    }
    
void Controller::setCommand(int n){
    this->modeList[currentMode]->finish();
    this->currentMode = n;
    this->modeList[currentMode]->initialize();
    }
    
void Controller::imuInt(){
    wait(0.25);
    this->modeList[currentMode]->execute();
    }

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