File content as of revision 12:0c10396d0615:

#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 float main_loop_wait_time = 0.01;

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

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

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


Motor main_motor(D6, D7, D13, D12);
Motor sec_motor(D5, D4, D10, D11);

// The motor -> main gear ratio is 25 / 60.
// The motor -> secondary gear ratio is 25/50
const float main_gear_ratio = 25.0/60.0;
const float sec_gear_ratio = 25.0/50.0;


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

// Which direction the emg will control the arm.
// Up or down.
// Left or right.
bool control_goes_up = false;
bool control_goes_right = false;


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()) {
        control_goes_up = !control_goes_up;
    }
    if (lr_button.has_just_been_pressed()) {
        control_goes_right = !control_goes_right;
    }
}

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("STOP");
    }

    // Stop the motors!
    main_motor.stop();
    sec_motor.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;
    
    screen.clear_display();
    
    main_motor.set_pid_k_values(Kp, Ki, Kd);
    sec_motor.set_pid_k_values(Kp, Ki, Kd);
    
    // One of the motors is reversed in the electronics.
    // This is fixed in the motor controll board, so we have to account
    // for it in software.
    main_motor.set_extra_reduction_ratio(main_gear_ratio);
    sec_motor.set_extra_reduction_ratio(-sec_gear_ratio);
    
    // Start the motor controller at the desired frequency.
    main_motor.start(pid_period);
    sec_motor.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);
    }
}