File content as of revision 0:2a5dd6cc0008:

#include "mbed.h"
#include "FastPWM.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "BiQuad.h"

enum States {WaitState, MotorCalState, EMGCalState, HomingState, MovingState, GrippingState, ScrewingState, FailureState};
States currentState = WaitState;

DigitalIn startButton(D0);
InterruptIn failureButton(D1);
DigitalIn grippingSwitch(D2);
DigitalIn screwingSwitch(D3);
MODSERIAL pc(USBTX, USBRX);
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);

bool stateChanged = true;

Ticker mainTicker;
Timer stateTimer;
double sampleTime = 0.01;

void switchToFailureState ()
{
    failureButton.fall(NULL); // Detaches button, so it can only be activated once
    pc.printf("SYSTEM FAILED");
    currentState = FailureState;
    stateChanged = true;
}

void stateMachine ()
{
    switch (currentState) {
        case WaitState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state
                // Set output motor to 0
                stateChanged = false;
            }

            if (startButton.read() == true) {
                pc.printf("Switching to motor calibration");
                led1 = 0;
                currentState = MotorCalState;
                stateChanged = true;
            }

            break;
        case MotorCalState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state
                led2 = 1;
                // Set motorpwm to 'low' value
                stateTimer.reset();
                stateTimer.start();
                stateChanged = false;
            }

            // Add stuff you do every loop

            if (stateTimer >= 3.0f) { //TODO: add && motor velocity < 0.1f
                pc.printf("Starting EMG calibration...\n");
                led2 = 0;
                currentState = EMGCalState;
                stateChanged = true;
            }
            break;
        case EMGCalState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state;
                led3 = 1;
                stateTimer.reset();
                stateTimer.start();
                stateChanged = false;
            }

            // Add stuff you do every loop

            if (stateTimer >= 3.0f) {
                pc.printf("Starting homing...\n");
                led3 = 0;
                currentState = HomingState;
                stateChanged = true;
            }
            break;
        case HomingState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state;
                led1 = 1;
                led2 = 1;
                // Set intended position
                stateChanged = false;
            }

            // Nothing extra happens till robot reaches starting position and button is pressed

            if (startButton.read() == true) { // Also add position condition
                pc.printf("Start moving...\n");
                led1 = 0;
                led2 = 0;
                currentState = MovingState;
                stateChanged = true;
            }
            break;
        case MovingState:
        if (stateChanged == true) {
                // Entry action: all the things you do once in this state;
                led1 = 1;
                led2 = 1;
                led3 = 1;
                // Set intended position
                stateChanged = false;
            }
            break;
        case GrippingState:
            break;
        case ScrewingState:
            break;
        case FailureState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state
                // Turn all motors off
                stateChanged = false;
                break;
            }
    }
}

void mainLoop ()
{
    // Add measure, motor controller and output function
    stateMachine();
}

int main()
{
    pc.baud(115200);
    mainTicker.attach(mainLoop, sampleTime);
    failureButton.fall(&switchToFailureState); // When button is pressed FailureState is activated

    while (true) {
        pc.printf("State = %i\n", currentState);
        wait(1);
    }
}