File content as of revision 6:bfc6e68774f5:

#include "mbed.h"

#include "Button.h"
#include "Screen.h"


enum States {waiting, calib_motor, calib_bicep1, calib_bicep2, homing, operation, failure}; // The possible states of the state machine

// Global variables
const double PI = 3.14159265359;
// Main loop wait time per cycle. This does not influence the motor PID or EMG reading frequencies.
const double main_loop_wait_time = 0.01;


States current_state;   // Defining the state we are currently in
States last_state;      // To detect state changes.
Ticker loop_ticker;     // Ticker for the loop function

// Order of buttons: up_down, left_right, panic
// D2, D3, D8
Button ud_button(PTA4);
Button lr_button(D3);
Button p_button(PTC6);

DigitalOut led_red(LED_RED);
DigitalOut led_green(LED_GREEN);

// The last arguent is the reset pin.
// The screen doesn't use it, but the library requires it
// So pick a pin we don't use.
Screen screen(D14, D15, D9);


void do_state_waiting()
{
    if(last_state != current_state) {
        last_state = current_state;
        // State just changed to this one.

        led_green = 1;
        screen.clear_display();
        screen.display_state_name("Waiting");
        screen.get_screen_handle()->printf("    Press to start   ");
        screen.get_screen_handle()->printf("          |          ");
        screen.get_screen_handle()->printf("          V          ");
        screen.display();
    }
    
    if (ud_button.is_pressed()) {
        current_state = calib_motor;
    }
}

void do_state_calib_motor()
{
    if(last_state != current_state) {
        last_state = current_state;
        // State just changed to this one.

        led_green = 0;
        screen.clear_display();
        screen.display_state_name("Motor calibration");
    }
}

void do_state_calib_bicep1()
{
    if(last_state != current_state) {
        last_state = current_state;
        // State just changed to this one.
        screen.clear_display();
        screen.display_state_name("EMG 1 calibration");
    }
}

void do_state_calib_bicep2()
{
    if(last_state != current_state) {
        last_state = current_state;
        // State just changed to this one.
        screen.clear_display();
        screen.display_state_name("EMG 2 calibration");
    }
}

void do_state_homing()
{
    if(last_state != current_state) {
        last_state = current_state;
        // State just changed to this one.
        screen.clear_display();
        screen.display_state_name("Homing");
    }
}

void do_state_operation()
{
    if(last_state != current_state) {
        last_state = current_state;
        // State just changed to this one.
        screen.clear_display();
        screen.display_state_name("Normal operation");
    }
}

void do_state_failure()
{
    if(last_state != current_state) {
        last_state = current_state;
        // State just changed.
        // Update the display.
        led_red = 0;
        led_green = 1;
        screen.clear_display();
        screen.display_state_name("Error!");
    }

    // Stop the motors!
}


void main_loop()
{
    ud_button.update();
    lr_button.update();
    p_button.update();

    switch (current_state) {
        case waiting:
            do_state_waiting();
            break;
        case calib_motor:
            do_state_calib_motor();
            break;
        case calib_bicep1:
            do_state_calib_bicep1();
            break;
        case calib_bicep2:
            do_state_calib_bicep2();
            break;
        case homing:
            do_state_homing();
            break;
        case operation:
            do_state_operation();
            break;
        case failure:
            do_state_failure();
            break;
    }

    // Check if the panic button was pressed.
    // Doesn't matter in which state we are, we need to go to failure.
    if (p_button.is_pressed()) {
        current_state = failure;
    }
}

int main()
{
    led_red = 1;

    // Start in the waiting state.
    current_state = waiting;
    // Pretend we come from the operation state.
    // So that the waiting state knows it just got started.
    last_state = operation;

    while (true) {
        main_loop();
        wait(main_loop_wait_time);
    }
}