File content as of revision 5:07e401cb251d:

#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};
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:
                // Description:
                // In this state we do nothing
                
                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:
                // Description:
                    // In this state the robot moves with low motor PWM to some 
                    // mechanical limit of motion.
                
                pc.printf("Motor angle calibration \r\n");
                led_red = 1; led_green = 0; led_blue = 0;   // Colouring the led TURQUOISE
                
                // Requirements to move to the next state:
                    // If enough time has passed in this state (3 sec) and the 
                    // motors stopped moving, we move to the EMG-calibration state.
                
                wait(3.0f);                                 // time_in_this_state > 3.0f
                // if (fabs(motor_velocity) < 0.01f) {
                // INSERT CALIBRATING
                currentState = EMG_CLBRT;
                break;
                
            case EMG_CLBRT:
                // In this state the person whom is connected to the robot needs 
                    // to flex his/her muscles as hard as possible, in order to 
                    // measure the maximum EMG-signal, which can be used to scale 
                    // the EMG-filter.
                
                pc.printf("EMG calibration \r\n");
                led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led BLUE
                
                // Requirements to move to the next state:
                    // If enough time has passed (5 sec), and the EMG-signal drops below 10%
                    // of the maximum measured value, we move to the Homing state.
                    
                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:
                // Description:
                    // Robot moves to the home starting configuration
                    
                pc.printf("HOMING, moving to the home starting configuration. \r\n");
                led_red = 0; led_green = 1; led_red = 0;                  // Colouring the led PURPLE
                
                // Requirements to move to the next state:
                    // If we are in the right location, within some margin, we move to the Waiting for
                    // signal state.
                
                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:
                // Description:
                    // In this state the robot waits for an action to occur.
                    
                led_red = 0; led_green = 0; led_blue = 0;                // Colouring the led WHITE
                
                // Requirements to move to the next state:
                    // If a certain button is pressed we move to the corresponding 
                    // state (MOVE_W_DEMO, MOVE_W_EMG or SHUTDOWN)
                
                // 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. \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;
                    }
  
                break;
                
                
            case MOVE_W_DEMO:
                // Description:
                    // In this state the robot follows a preprogrammed shape, e.g. 
                    // a square.
                    
                led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led GREEN
                pc.printf("MOVE_W_DEMO, Running the demo \r\n");
                
                // Requirements to move to the next state:
                    // When the home button or the failure button is pressed, we 
                    // will the move to the corresponding state.
                
                // 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:
                // Description:
                    // In this state the robot will be controlled by use of 
                    // EMG-signals. 
                    
                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");
                
                // Requirements to move to the next state:
                    // When the home button or the failure button is pressed, we 
                    // will the move to the corresponding state.
                
                // 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:
                // Description:
                // This state is reached when the failure button is reached. 
                // In this state everything is turned off.
            
                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;

                        
            default: 
                // This state is a default state, this state is reached when 
                // the program somehow defies all of the other states. 
                
                pc.printf("Unknown or unimplemented state reached!!! \n\r");
                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
    }
}