File content as of revision 26:21911b4a79b4:

//Libraries
    #include "mbed.h"
    #include "FastPWM.h"
    #include "HIDScope.h"
    #include "MODSERIAL.h"
    #include "QEI.h"
    
//Homebrew libraries
    #include "header.h"
    //#include "controller.cpp"


//Objects
    //LED
        DigitalOut ledb(LED_BLUE); // ledb=true & ledb=1 is led off!!
        DigitalOut ledg(LED_GREEN);
        DigitalOut ledr(LED_RED);
    //Motors
        
    //Sensors
        //QEI Encoder1(D12,D13,NC,64,QEI::X2_ENCODING);
        //QEI Encoder2(D14,D15,NC,64,QEI::X2_ENCODING);
        //QEI Encoder3(D16,D17,NC,64,QEI::X2_ENCODING);
        
    //Buttons
        InterruptIn button1(SW2); //button on the side of the reset button
        InterruptIn button2(SW3); //button on the side opposite of the reset button

    //PC
        Serial pc(USBTX,USBRX);
        
    //Structure
        struct struc1
        {
            int calibrationCountsMotor1;
            int calibrationCountsMotor2;
            int calibrationCountsMotor3;
            
            float countsMotor1Return;
            float countsMotor2Return;
            float velocityMotor1Return;
            float velocityMotor2Return;
        };
        struc1 strucM&E;

//Variables
    enum States {MovementIdle, CalibrationIdle, Demo, Startup, CalibrationPhysical, CalibrationEMG, Move, TiltCup, FailState};
    States CurrentState; 

    volatile char ledcolor; //r is red, b is blue, g is green, t is bluegreen, p is purple
    volatile int errorCode;
    
        
    //Ticker Timings
        const float mainLoopT = 2; //Main Loopt Ticker wait
        const float ledFlipperT = .5; //LED Flicker wait
    


//Tickers
    Ticker Main_Ticker;
    Ticker Tick_Blinky;//used for the blinking of the leds

//Led FLicker
void FlipLED(void)
{
    pc.printf("FLIPLED \r\n");
    switch(ledcolor)
    {
        case 'r':
            ledr = !ledr;
            ledg = true;
            ledb = true;
            break;
        case 'b':
            ledr = true;
            ledg = true;
            ledb = !ledb;
            break;
        case 'g':
            ledr = true;
            ledg = !ledg;
            ledb = true;
            break;
        case 't':
            ledr = true;
            ledg = !ledg;
            ledb = !ledb;
            break;
        case 'p':
            ledr = !ledr;
            ledg = true;
            ledb = !ledb;
            break;
        default:
            errorCode = 1;
            CurrentState = FailState;
    }
    pc.printf("Color %c \r\n",ledcolor);
}

// SW2 = button1 state changing
void Run_StateChangerButton1()
{
    switch(CurrentState)
    {
    case Startup: //From
        CurrentState = CalibrationPhysical; //To
        break; //Break from switch
    case CalibrationPhysical:
        CurrentState = CalibrationIdle;
        break;
    case CalibrationIdle:
        CurrentState = CalibrationEMG;
        break;
    case CalibrationEMG:
        CurrentState = MovementIdle;
        break;
    case MovementIdle:
        CurrentState = TiltCup;
        break;
    case TiltCup:
        CurrentState = MovementIdle;
        break;
    case Move:
        CurrentState = MovementIdle;
        break;
    }
}
// SW3 = button2 state changing
void Run_StateChangerButton2(void)
{
    switch(CurrentState)
    {
        case CalibrationIdle:
            CurrentState = Demo;
            break;
        case MovementIdle:
            CurrentState = Move;
            break;
        case TiltCup:
            CurrentState = MovementIdle;
            break;
    }
}


//State functions
void Run_Demo(void)
{
    pc.printf("Starting Demo ...\r\n");
}

void Run_MovementIdle(void)
{
    pc.printf("Starting Idle ...\r\n");
}

void Run_CalibrationIdle(void)
{
    pc.printf("Starting Calibration Idle ...\r\n");
}

void Run_Startup(void)
{
    pc.printf("Starting Startup ...\r\n");
}

void Run_CalibrationPhysical(void)
{
    pc.printf("Starting Calibration Physical ... \r\n");
    float calibrationPeriodMotor1 = 1/2000; 
    float calibrationPeriodMotor2 = 1/2000;
    float calibrationPeriodMotor3 = 1/2000; 
    
    float calibrationPWM = .1;
    float calibrationVelocity = .1;
    
    float calibrationPWM1 = calibrationPWM;
    float calibrationPWM2 = 0;
    float calibrationPWM3 = 0;
    
    motorAndEncoder(calibrationPWM1, calibrationPeriodMotor1, calibrationPWM2, calibrationPeriodMotor2, mainLoopT,  &countsMotor1Return, &countsMotor2Return, &velocityMotor1Return, &velocityMotor2Return, calibrationCountsMotor1, calibrationCountsMotor2, calibrationCountsMotor3);
    
    if (velocityMotor1 <= calibrationVelocity)
    {
        calibrationCountsMotor1 = countsMotor1Return; //calibrate motor1
        calibrationPWM1 = 0;
        calibrationPWM2 = calibrationPWM;
        calibrationPWM3 = 0;
        if (velocityMotor2 <= calibrationVelocity) //Calibrate motor2
        {
            calibrationCountsMotor2 = countsMotor2Return;
            calibrationPWM1 = 0;
            calibrationPWM2 = 0;
            calibrationPWM3 = calibrationPWM;
            
            if (velocityMotor3 <= calibrationVelocity) //calibrate motor3
            {
                calibrationCountsMotor3 = countsMotor3Return;
                calibrationPWM1 = 0;
                calibrationPWM2 = 0;
                calibrationPWM3 = 0;
                motorAndEncoder(calibrationPWM1, calibrationPeriodMotor1, calibrationPWM2, calibrationPeriodMotor2, mainLoopT,  float &countsMotor1Return, float &countsMotor2Return, float &velocityMotor1Return, float &velocityMotor2Return, calibrationCountsMotor1, calibrationCountsMotor2, calibrationCountsMotor3);
            }
        }
        
    }
    
    
    CurrentState = CalibrationIdle;
    
}

void Run_CalibrationEMG(void)
{
    pc.printf("Starting Calibration EMG ... \r\n");
}

void Run_Move(void)
{
    pc.printf("Starting Move ... \r\n");
}

void Run_TiltCup(void)
{
    pc.printf("Starting Calibration TiltCup ... \r\n");
}

void Run_FailState(void)
{   
    pc.printf("Error: %i",errorCode);
}


//State Machine
void StateMachine(void)
{
    //Turn off all LEDs    
    switch(CurrentState)
    {
        case Demo:
            ledcolor='t';
            Run_Demo();
            break;
        case MovementIdle:
            ledcolor='b';
            Run_MovementIdle();
            break;
        case CalibrationIdle:
            ledcolor='b';
            Run_CalibrationIdle();
            break;
        case Startup:
            ledcolor='b';
            Run_Startup();
            break;
        case CalibrationPhysical:
            ledcolor='g';
            Run_CalibrationPhysical();
            break;
        case CalibrationEMG:
            ledcolor='g';
            Run_CalibrationEMG();
            break;
        case Move:
            ledcolor='p';
            Run_Move();
            break;
        case TiltCup:
            ledcolor='t';
            Run_TiltCup();
            break;
        case FailState:
            ledcolor='r';
            Run_FailState();
            break;
        default:
            CurrentState = FailState;
            errorCode = 2;
            break;
    }
}

//Main Loop
void mainloop()
{
        StateMachine(); 
}

int main()
{   
    //Initialize
    ledr = true;
    //ledr.write(.4);
    ledg = true;
    //ledg.write(.4);
    ledb = true;
    //ledb.write(.4);
    
    pc.baud(115200);    
    CurrentState = Startup;
    
    button1.mode(PullUp);
    button1.rise(Run_StateChangerButton1);
    button2.mode(PullUp);
    button2.rise(Run_StateChangerButton2);
    
    //Tickers
        Main_Ticker.attach(mainloop,mainLoopT);
        wait(mainLoopT);
        Tick_Blinky.attach(FlipLED,ledFlipperT);
    
    //Placeholder function call
    double PlantError = 1;
    double Ts = 1;
    double u = ControllerPID(PlantError, Ts);
    
    while(true)
    {

    }
}