File content as of revision 0:50f25a675c72:

// Robot states
#include "mbed.h"
#include "MODSERIAL.h"

// Define objects
DigitalOut led1(LED_RED);
DigitalOut led2(LED_BLUE);
DigitalOut led3(LED_GREEN);
DigitalIn ptc1(PTC6); //Button 1
DigitalIn ptc2(PTA4); //Button 2

//v_motor, t_passed, EMG zijn allemaal inputs die we nog even moeten fiksen ik weet niet hoe
// Functions 
enum states{START,KAL_ME,KAL_EMG,MOVE_START,READY_START,DEMO,MOVE,WAIT,OFF};

states CurrentState(START);
bool StateChanged true;         // this is the initialization of the first state

void StateMachine(void)
{
    switch(CurrentState)
    {
        case START:
            while(true)     //RED LED ON
            {
                led2 = 1;
                led3 = 1;
                led1 = !led1;   
            }
            
            if (ptc1 == 0) // State switches when button pressed
            {
                CurrentState = KAL_ME;
                StateChanged = true;
            }
        break;      // end of state START
                
        case KAL_ME:
            while(true)
            {
                led2 = 1;
                led3 = 1;
                led1 = !led1;
                wait(0.7);
            }
            if  (v_motor == 0 && t_passed = 2) //sec
            {
                CurrentState = KAL_EMG;
                StateChanged = true;
            }
        break;      // end of state KAL_ME
        
        case KAL_EMG:
            while(true)
            {
                led2 = 1;
                led3 = 1;
                led1 = !led1;
                wait(0.2);
            }
            
            while(t_passed < 5) //sec
            {
                //EMG_max = max EMG measured in these 5 sec
            }
            
            if (EMG <= 0.1*EMG_max && t_passed == 2) //sec
            {
                CurrentState = MOVE_START;
                StateChanged = true;
            }
        break;      // end of state KAL_EMG
        
        case MOVE_START:
            while(true)
            {
                led1 = 1;
                led2 = 1;
                led3 = !led3;
                wait(0.2);
            }
            
            // define start position
            
            if (t_passed == 2 && //position realised and tmie passed
            {
                // Stop moving
                CurrentState = READY_START;
                StateChanged = true;
            } 
        break;      // end of state MOVE_START
        
        case READY_START:
            while(true)
            {
                led1 = 1;
                led2 = 1;
                led3 = 0;
            }
            
            if (ptc1 == 0) 
            {
                CurrentState = DEMO;
                StateChanged = true;
            }
            else if (ptc2 == 0) // or EMG signal >= 20% max
            {
                CurrentState = MOVE;
                StateChanged = true;
            }                
        break;      // end of state READY_START
        
        case DEMO:
            //perform straight movements in some way, dunno how to make that here yet
            if //position = end and time 
            {
                CurrentState = MOVE_START;
                StateChanged = true;
            }
        break;      // end of state DEMO
        
        case MOVE:
            if // no movement for 2 sec after input
            {
                CurrentState = WAIT;
                StateChanged = true;
            }
            else if // input signal
            {
                // move in direction linked to EMG
            }
            // Iets met dat oranje lichtje maar snap die stap niet helemaal 
        break;      // end of state MOVE
        
        case WAIT:
            if (ptc1 == 0)
            {
                CurrentState = READY_START;
                StateChanged = true;
            )
            else if (ptc2 == 0) // or EMG>= 20%max
            {
                CurrentState = MOVE;
                StateChanged = true;
            }
            else if (ptc1 == 0) //maar dan heel lang achter elkaar
        break;      // end of state WAIT
        
        case OFF:
            // turns all of 
        break;      // end of state OFF
        
        default:
        TurnMotorsoff();
        printf("Unknown state reached");
    }               // End of the switch, all states are prescribed
}

int main(void)      // wat hier in moet snap ik nog niet 
{
        // hier moeten dingen komen
        while (true)
        {
            CheckForCommandFromTerminal();
            StateMachine();
        }
}