File content as of revision 4:a0c1c021026b:

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

DigitalOut led_red(LED_RED);
DigitalOut led_green(LED_GREEN);
DigitalOut led_blue(LED_BLUE);
DigitalIn button(SW2);
InterruptIn Fail_button(SW3);
MODSERIAL pc(USBTX, USBRX);

enum states {WAITING, MOTOR_ANGLE_CLBRT, EMG_CLBRT, HOMING, WAITING4SIGNAL, MOVE_W_EMG, MOVE_W_DEMO, FAILURE_MODE, SHUTDOWN};
states currentState = WAITING;
char c;
    
void Failing() {
    currentState = FAILURE_MODE;
    wait(1.0f);
    }

int main()
{
    // Switch all LEDs off
    led_red = 1;
    led_green = 1;
    led_blue = 1;
    pc.baud(115200);
    Fail_button.fall(&Failing);    

    pc.printf("\r\n______________ STATE MACHINE ______________ \r\n\r\n");
    
    while (true) {
        switch (currentState)
        {
            case WAITING:
                pc.printf("WAITING... \r\n");
                led_red = 0; led_green = 0; led_blue = 0;   // Colouring the led WHITE
                if (!button) {
                    currentState = MOTOR_ANGLE_CLBRT;
                    }
                wait(1.0f);
                break;
                
            case MOTOR_ANGLE_CLBRT:
                pc.printf("Motor angle calibration \r\n");
                led_red = 1; led_green = 0; led_blue = 0;   // Colouring the led TURQUOISE
                wait(3.0f);                                 // time_in_this_state > 3.0f
                // if (fabs(motor_velocity) < 0.01f) {
                // INSERT CALIBRATING
                currentState = EMG_CLBRT;
                break;
                
            case EMG_CLBRT:
                pc.printf("EMG calibration \r\n");
                led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led BLUE
                wait(5.0f); // time_in_this_state > 5.0f
                // if moving_average(procd_emg) < 0.1*max_procd_emg_ever
                // INSERT CALIBRATING
                currentState = HOMING;
                break;       
                            
            case HOMING:
                pc.printf("HOMING, moving to the home starting configuration. \r\n");
                led_red = 0; led_green = 1; led_red = 0;                  // Colouring the led PURPLE
                wait(5.0f); // time_in_this_state > 5.0f
                // if ((fabs(angle_error1) < 0.01f) && (fabs(angle_error2) < 0.01f)) {
                // INSERT MOVEMENT
                currentState = WAITING4SIGNAL;
                break;
                
            case WAITING4SIGNAL:
                led_red = 0; led_green = 0; led_blue = 0;                // Colouring the led WHITE
                // CANNOT USE KEYBOARD. SO LOOK FOR ALTERNATIVE
                pc.printf("Press d to run the demo, Press e to move with EMG, Press c to re-calibrate or Press s to shut the robot down. \r\n");
                c = pc.getc();
                if (c == 'd') {
                    currentState = MOVE_W_DEMO;
                    }
                else if (c == 'e') {
                    currentState = MOVE_W_EMG;
                    }
                else if (c == 'c') {
                    currentState = MOTOR_ANGLE_CLBRT;
                    }
                else if (c == 's') {
                    currentState = SHUTDOWN;
                    }
                break;
                
                
            case MOVE_W_DEMO:
                led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led GREEN
                pc.printf("MOVE_W_DEMO, Running the demo \r\n");
                // BUILD DEMO MODE
                // FIND ALTERNATIVE FOR KEYBOARD
                c = pc.getcNb();
                if (c == 'h') {
                    currentState = HOMING;
                    }
                Fail_button.fall(&Failing); 
                wait(1.0f);
                break;
                
            case MOVE_W_EMG:
                led_red = 1; led_green = 0; led_blue = 1;   // Colouring the led GREEN
                pc.printf("MOVE_W_EMG, Moving with use of EMG \r\n");
                // BUILD THE MOVEMENT WITH EMG
                wait(1);
                c = pc.getcNb();
                if (c == 'h') {
                    currentState = HOMING;
                    }
                Fail_button.fall(&Failing); 
                wait(1.0f);
                break;      
                 
            case FAILURE_MODE:
                led_red = 0; led_green = 1; led_blue = 1;   // Colouring the led RED
                // WHAT NEEDS TO HAPPEN IN FAILURE MODE?
                pc.printf("FAILURE MODE \r\n");
                wait(2.0f);
                break;
                
            case SHUTDOWN:
                pc.printf("Shutting down \r\n");
                // BUILD WHAT NEEDS TO BE DONE WHEN SHUTTING DOWN
                led_red = 1; led_green = 1; led_blue = 1;   // Colouring the led BLACK
                wait(1.0f);
                for (int i = 0; i < 4; i++) {               // Blinking WHITE 3x before turning BLACK
                    led_red = !led_red;
                    led_green = !led_green;
                    led_blue = !led_blue;
                    wait(0.6f);
                    }
                    led_red = !led_red;
                    led_green = !led_green;
                    led_blue = !led_blue;
                    wait(1.0f);
                    led_red = !led_red;
                    led_green = !led_green;
                    led_blue = !led_blue;
                while (true) {
                    }

                        
            default: 
                pc.printf("Unknown or unimplemented state reached!!! \n\r");
                // WHAT HAPPENS IN THIS STATE? DOES IT KEEPS MOVING?
                led_red = 1; led_green = 1; led_blue = 1;   // Colouring the led BLACK
                for (int n = 0; n < 50; n++) {              // Making an SOS signal with the RED led
                    for (int i = 0; i < 6; i++) { 
                        led_red = !led_red;
                        wait(0.6f);
                        }
                    wait(0.4f);
                    for (int i = 0 ; i < 6; i++) {
                        led_red = !led_red;
                        wait(0.2f);
                        }
                    wait(0.4f);
                }
        }   // end of switch
    }
}