File content as of revision 2:141cfcafe72b:

#include "mbed.h"

#include "Button.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);


void do_state_waiting() {
    if (ud_button.is_pressed()) {
        current_state = calib_motor;
    }
    
    led_green = 1;
}

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

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

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

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

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

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;
    }
    
    // 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;
    last_state = waiting;
    
    while (true) {
        main_loop();
        wait(main_loop_wait_time);
    }
}