File content as of revision 1:4cb9af313c26:

#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(SW2);
DigitalIn screwingSwitch(SW3);
DigitalIn gripDirection(D2);
DigitalIn screwDirection(D3);
MODSERIAL pc(USBTX, USBRX);
DigitalOut led1(LED1); // Red led
DigitalOut led2(LED2); // Green led
DigitalOut led3(LED3); // Blue led

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
    led1 = 0;
    led2 = 1;
    led3 = 1;
    pc.printf("SYSTEM FAILED\n");
    currentState = FailureState;
    stateChanged = true;
}

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

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

            break;
        case MotorCalState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state
                led2 = 0;
                // 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
                led2 = 1;
                currentState = EMGCalState;
                stateChanged = true;
            }
            break;
        case EMGCalState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state;
                led3 = 0;
                stateTimer.reset();
                stateTimer.start();
                stateChanged = false;
            }

            // Add stuff you do every loop

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

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

            if (startButton.read() == false) { //TODO: Also add position condition
                led1 = 1;
                led2 = 1;
                currentState = MovingState;
                stateChanged = true;
            }
            break;
        case MovingState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state;
                led1 = 0;
                led2 = 0;
                led3 = 0; // Emits white together
                stateChanged = false;
            }

            if (grippingSwitch.read() == false) {
                led1 = 1;
                led2 = 1;
                led3 = 1;
                currentState = GrippingState;
                stateChanged = true;
            }

            break;
        case GrippingState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state;
                led2 = 0;
                led3 = 0; // Emits x together
                stateChanged = false;
            }

            if (gripDirection == true) {
                // Close gripper
            } else {
                // Open gripper
            }

            if (screwingSwitch.read() == false) {
                led2 = 1;
                led3 = 1;
                currentState = ScrewingState;
                stateChanged = true;
            }
            if (startButton.read() == false) {
                led2 = 1;
                led3 = 1;
                currentState = MovingState;
                stateChanged = true;
            }
            break;
        case ScrewingState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state;
                led1 = 0;
                led3 = 0; // Emits pink together
                stateChanged = false;
            }
            
            if (screwDirection == true) {
                // Screw
            } else {
                // Unscrew
            }
            
            if (startButton.read() == false) {
                led1 = 1;
                led3 = 1;
                currentState = MovingState;
                stateChanged = true;
            }
            break;
        case FailureState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state
                //TODO: Turn all motors off
                stateChanged = false;
            }

            static double blinkTimer = 0;
            if (blinkTimer >= 0.5) {
                led1 = !led1;
                blinkTimer = 0;
            }
            blinkTimer += sampleTime;

            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);
    }
}