File content as of revision 3:8be18839d069:

#include "mbed.h"
#include "MODSERIAL.h"

DigitalOut ledr(LED1);
DigitalOut ledg(LED2);
DigitalOut ledb(LED3);
DigitalIn button1(SW2);
DigitalIn button2(SW3);
MODSERIAL pc(USBTX, USBRX);
enum states{WAITING, EMG_CAL, BCM, FAIL};
states CurrentState = WAITING;
bool StateChanged = true;
volatile int x = 0;
bool failure = false;

void ProcessStateMachine(void)
{
    switch(CurrentState)
    {
        case WAITING: //This state is used to only start calibrating upon user input
            //State initialization
            if(StateChanged) 
            {
                pc.printf("Entering WAITING\r\n");
                ledr = 0;
                ledg = 1;
                ledb = 1;
                StateChanged = false;
            }
            //State actions
            pc.printf("Still WAITING\r\n");
            wait(1.0f); 
            while(button1)
            {
            }
            //State transition  
            CurrentState = EMG_CAL;
            StateChanged = true;
            break; // End of WAITING
        case EMG_CAL: //State for calibrating EMG channels
            //State initialization
            if(StateChanged) 
            {
                pc.printf("Entering EMG calibration\r\n");
                x = 0;
                ledr = 1;
                ledg = 0;
                ledb = 1;
                StateChanged = false;
            }
            //State actions
            pc.printf("EMG calibration starting\r\n");
            wait(0.3f); //Dit moet nog veranderd worden naar timer die opneemt hoe lang hij in WAITING zit.
            // calibratiestappen voor de emg hier invoegen
            while(x < 4 && !failure)
            {
                if(x == 1)
                {
                    //Functie die het eerste EMG signaal calibreert
                }
                if(x == 2)
                {
                    //Functie die het tweede  EMG signaal calibreert
                }
                if(x == 3)
                {
                    //Functie die het derde EMG signaal calibreert
                }
                if(!button2)
                {
                    failure = true;
                }
                if(!button1)
                {
                    x++;
                    pc.printf("EMG calibration %i is now running\r\n", x);
                    wait(0.2f);
                }
            }
            //State transition
            if(failure)
            {
                CurrentState = FAIL;
                StateChanged = true;
            }
            else
            {
                CurrentState = BCM;
                StateChanged = true;
            }
            break; //End of EMG_CAL state
        case BCM: //State for moving the robot
            //State initialization
            if(StateChanged)
            {
                pc.printf("Entering Basic Control Mode\r\n");
                ledr = 1;
                ledg = 1;
                ledb = 0;
                StateChanged = false;
            }
            //State actions
            pc.printf("Basic Control Mode is active\r\n");
            while(button1 && !failure)
            {
                if(!button2)
                {
                    failure = true;
                }
            }
            //State transition
            if(failure)
            {
                pc.printf("Go to fail mode\r\n");
                CurrentState = FAIL;
                StateChanged = true;
            }
            else
            {
                CurrentState = WAITING;
                StateChanged = true;
            }
            break; //End of BCM state
        case FAIL: //Fail state
            //State initialization
            if(StateChanged)
            {
                pc.printf("Fail mode is entered\r\n");
                failure = false;
                StateChanged = false;
            }
            //State actions
            pc.printf("Fail mode is active. Press button1 to enter WAITING\r\n");
            while(button1)
            {
            }
            //State transition
            CurrentState = WAITING;
            StateChanged = true;
            break; //End of FAIL state
        default: //Emergency state for when something unexpected happens
            //motoren uit enzo
            pc.printf("SOS SOS SOS\r\n");
            ledr = 0;
            ledg = 0;
            ledb = 0;
            while(button1)
            {
                ledr = !ledr;
                wait(0.3f);
            }
            CurrentState = WAITING;
            StateChanged = true;
    }
}

int main() 
{
    pc.baud(115200);
    ledr = 1;
    ledg = 1;
    ledb = 1;
    while (true) 
    {
        ProcessStateMachine(); //Activates state machine
    }
}