File content as of revision 9:27d00b64076e:

#include "mbed.h"

#include "Button.h"
#include "Screen.h"
#include "motor.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;

// Time between two button polls. Used to debounce the button presses.
const double button_poll_interval = 0.1;

const float pid_period = 0.001; // PID sample period in seconds.

const double Kp = 10.0;
const double Ki = 0.1;
const double Kd = 0.5;


Motor motor1(D6, D7, D13, D12);
Motor motor2(D5, D4, D10, D11);


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(D2);
Button lr_button(D3);
Button p_button(D8);

Ticker button_ticker;

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

// 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.has_just_been_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");
    }
    
    if (ud_button.has_just_been_pressed()) {
        current_state = calib_bicep1;
    }
}

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");
    }
    
    if (ud_button.has_just_been_pressed()) {
        current_state = calib_bicep2;
    }
}

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");
    }
    
    if (ud_button.has_just_been_pressed()) {
        current_state = homing;
    }
}

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");
    }
    
    if (ud_button.has_just_been_pressed()) {
        current_state = operation;
    }
}

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");
    }
    
    if (ud_button.has_just_been_pressed()) {
        current_state = homing;
    }
}

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!
    motor1.stop();
    motor2.stop();
}


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;
    }
}

void poll_buttons() {
    // We need to poll the pins periodically.
    // Normally one would use rise and fall interrupts, so this wouldn't be
    // needed. But the buttons we use generate so much chatter that
    // sometimes a rising or a falling edge doesn't get registered.
    // With all the confusion that accompanies it.
    ud_button.poll_pin();
    lr_button.poll_pin();
    p_button.poll_pin();
}

int main()
{
    led_red = 1;
    led_green = 1;
    led_blue = 1;
    
    motor1.set_pid_k_values(Kp, Ki, Kd);
    motor2.set_pid_k_values(Kp, Ki, Kd);
    // Start the motor controller at the desired frequency.
    motor1.start(pid_period);
    motor2.start(pid_period);

    // 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;
    
    // Start the button polling ticker.
    button_ticker.attach(&poll_buttons, button_poll_interval);

    while (true) {
        main_loop();
        
        // Button debugging.
        if (ud_button.has_just_been_pressed() || lr_button.has_just_been_pressed() || p_button.has_just_been_pressed()) {
            led_blue = 0;
        } else {
            led_blue = 1;
        }
        
        wait(main_loop_wait_time);
    }
}