File content as of revision 44:0056118c01b2:

#include "mbed.h"

#include "constants.h"

#include "Button.h"
#include "Screen.h"
#include "motor.h"
#include "EMGFilter.h"
#include "motor_calibration.h"
#include "forward_kinematics.h"
#include "inverse_kinematics.h"
#include "end_effector_control.h"
#include "EMG_calibration.h"


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



// Controll directions for the demo controller.
enum DebugControlDirection {debug_up, debug_down, debug_left, debug_right};


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


AnalogIn potmeter1(A5); // Analoge input van potmeter 1 -> Motor 1
AnalogIn potmeter2(A4); // Analoge input van potmeter 2 -> Motor 2

EMGFilter emg_1(A0);
EMGFilter emg_2(A1);


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()
{
    static double main_last_angle;
    static double sec_last_angle;
    static int main_iterations_not_moving;
    static int sec_iterations_not_moving;
    static bool main_is_calibrated;
    static bool sec_is_calibrated;

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

        led_green = 1;
        led_blue = 1;
        screen.clear_display();
        screen.display_state_name("Motor calibration");

        main_last_angle = -10;
        sec_last_angle = -10;
        main_iterations_not_moving = 0;
        sec_iterations_not_moving = 0;
        main_is_calibrated = false;
        sec_is_calibrated = false;
    }

    if (!main_is_calibrated) {
        led_green = 1;
        main_is_calibrated = calibrate_motor(main_motor, main_last_angle, main_iterations_not_moving);
        if (main_is_calibrated) {
            main_motor.define_current_angle_as_x_radians(main_motor_calibration_angle);
            //main_motor.set_target_angle(main_motor_calibration_angle - 0.2); // Give the arm some breathing space.
            led_green = 0;
        }
    }
    if (!sec_is_calibrated) {
        led_blue = 1;
        sec_is_calibrated = calibrate_motor(sec_motor, sec_last_angle, sec_iterations_not_moving);
        if (sec_is_calibrated) {
            sec_motor.define_current_angle_as_x_radians(sec_motor_calibration_angle); // -42 degrees.
            //main_motor.set_target_angle(sec_motor_calibration_angle + 0.2); // Give the arm some breathing space.
            led_blue = 0;
        }
    }

    screen.get_screen_handle()->setTextCursor(0, 8);
    screen.get_screen_handle()->printf("M: %i    \n", main_iterations_not_moving);
    screen.get_screen_handle()->printf("S: %i    \n", sec_iterations_not_moving);
    screen.display();

    if (main_is_calibrated && sec_is_calibrated) {
        current_state = homing;
    }
}

void do_state_homing()
{
    const double home_x = 0.6524; // Meters.
    const double home_y = 0.3409;

    double main_home;
    double sec_home;

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

        inverse_kinematics(home_x, home_y, main_home, sec_home);

        main_motor.set_target_angle(main_home);
        sec_motor.set_target_angle(sec_home);

        screen.get_screen_handle()->setTextCursor(0, 8);
        screen.get_screen_handle()->printf("Ma: %.6f    \n", main_home);
        screen.get_screen_handle()->printf("Sa: %.6f    \n", sec_home);
        screen.display();
    }

    if (ud_button.has_just_been_pressed()) {
        current_state = calib_bicep1;
    }
    if (lr_button.has_just_been_pressed()) {
        current_state = demo;
    }
}

void do_state_calib_bicep1()
{
    static EMG_calibration calibration = EMG_calibration(&screen, &emg_1);

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

        calibration.start();
    }

    if (ud_button.has_just_been_pressed() && calibration.is_calibrated()) {
        current_state = calib_bicep2;
    }
}

void do_state_calib_bicep2()
{
    static EMG_calibration calibration = EMG_calibration(&screen, &emg_2);

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

        calibration.start();
    }

    if (ud_button.has_just_been_pressed() && calibration.is_calibrated()) {
        current_state = operation;
    }
}

void do_state_operation()
{
    static bool debug_forward_kinematics;
    
    static const double max_speed = 0.01;
    static double speed_x;
    static double speed_y;
    
    static bool last_state_1;
    static bool last_state_2;

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

        control_goes_up = true;
        control_goes_right = true;
        
        speed_x = 0;
        speed_y = 0;

        screen.display_up_down_arrow(control_goes_up);
        screen.display_left_right_arrow(control_goes_right);

 //       debug_forward_kinematics = true;
        
        last_state_1 = false;
        last_state_2 = false;
    }
    
    bool emg_1_state = emg_1.get_is_envelope_over_threshold();
    bool emg_2_state = emg_2.get_is_envelope_over_threshold();
    
    if (emg_1_state && !last_state_1) {
        led_green = 0;

        if (control_goes_up) {
            speed_y = max_speed;
        } else {
            speed_y = -max_speed;
        }
    } else {
        led_green = 1;
        speed_y = 0;
    }
    
    if (emg_2_state && !last_state_2) {
        led_blue = 0;
        if (control_goes_right) {
            speed_x = max_speed;
        } else {
            speed_x = -max_speed;
        }
    } else {
        led_blue = 1;
        speed_x = 0;
    }
    
    last_state_1 = emg_1_state;
    last_state_2 = emg_2_state;

    /*
    if (debug_forward_kinematics) {
        // Using potmeters for debugging purposes;
        double main_angle = ((potmeter1.read() * 2) - 1) * PI;
        double sec_angle = ((potmeter2.read() * 2) - 1) * PI;

        double e_x = 0.0;
        double e_y = 0.0;

        forward_kinematics(main_angle, sec_angle, e_x, e_y);

        screen.get_screen_handle()->setTextCursor(0, 0);
        screen.get_screen_handle()->printf("M_a: %.6f    \n", main_angle);
        screen.get_screen_handle()->printf("S_a: %.6f    \n", sec_angle);
        screen.get_screen_handle()->printf("X:   %.6f    \n", e_x);
        screen.get_screen_handle()->printf("Y:   %.6f    ", e_y);
        screen.display();

    } else {
        // Using potmeters for debugging purposes;
        double e_x = potmeter1.read();
        double e_y = potmeter2.read();

        double main_angle = 0.0;
        double sec_angle = 0.0;

        inverse_kinematics(e_x, e_y, main_angle, sec_angle);

        screen.get_screen_handle()->setTextCursor(0, 0);
        screen.get_screen_handle()->printf("E_x: %.6f    \n", e_x);
        screen.get_screen_handle()->printf("E_y: %.6f    \n", e_y);
        screen.get_screen_handle()->printf("M_a: %.6f      \n", main_angle);
        screen.get_screen_handle()->printf("S_a: %.6f      ", sec_angle);
        screen.display();
    }

    if (lr_button.has_just_been_pressed()) {
        debug_forward_kinematics = !debug_forward_kinematics;
    }*/
    
    
    /*
    double main_target = ((potmeter1.read() * 2) - 1) * PI;
    main_motor.set_target_angle(main_target);
    double sec_target = ((potmeter2.read() * 2) - 1) * PI;
    sec_motor.set_target_angle(sec_target);

    if (lr_button.has_just_been_pressed()) {
    control_goes_right = !control_goes_right;
    screen.display_left_right_arrow(control_goes_right);
    }
    */
    
    double main_cur_angle = main_motor.get_current_angle();
    double sec_cur_angle = sec_motor.get_current_angle();

    double main_target, sec_target;

    end_effector_control(speed_x, speed_y, main_cur_angle, sec_cur_angle, main_target, sec_target);

    main_motor.set_target_angle(main_target);
    sec_motor.set_target_angle(sec_target);

    /*
    screen.get_screen_handle()->setTextCursor(0, 0);
    screen.get_screen_handle()->printf("M_a: %.6f    \n", main_cur_angle);
    screen.get_screen_handle()->printf("S_a: %.6f    \n", sec_cur_angle);
    screen.get_screen_handle()->printf("Vx:  %.6f \n", main_target);
    screen.get_screen_handle()->printf("Vy:  %.6f ", sec_target);
    screen.display();
    */
    
    screen.display_emg_state(emg_1_state, emg_2_state);
    screen.display();

    if (ud_button.has_just_been_pressed()) {
        control_goes_up = !control_goes_up;
        screen.display_up_down_arrow(control_goes_up);
    }

    if (lr_button.has_just_been_pressed()) {
        control_goes_right = !control_goes_right;
        screen.display_left_right_arrow(control_goes_right);
    }
}

void do_state_demo()
{
    static DebugControlDirection control_direction;
    static const double max_speed = 0.01;
    static double speed_x;
    static double speed_y;

    if(last_state != current_state) {
        last_state = current_state;
        // State just changed.
        // Update the display.
        led_red = 1;
        led_green = 0;
        led_blue = 1;
        screen.clear_display();
        screen.display_state_name("Demo mode!");

        control_direction = debug_up;

        speed_x = 0;
        speed_y = 0;

        screen.display_up_down_arrow(control_goes_up);
        screen.display_left_right_arrow(control_goes_right);
    }

    if (lr_button.has_just_been_pressed()) {
        switch (control_direction) {
            case debug_up:
                control_direction = debug_right;
                speed_x = max_speed;
                speed_y = 0;
                break;
            case debug_right:
                control_direction = debug_down;
                speed_x = 0;
                speed_y = -max_speed;
                break;
            case debug_down:
                control_direction = debug_left;
                speed_x = -max_speed;
                speed_y = 0;
                break;
            case debug_left:
                control_direction = debug_up;
                speed_x = 0;
                speed_y = max_speed;
                break;
        }
    }

    if (ud_button.is_pressed()) {

        led_blue = 0;


        double main_cur_angle = main_motor.get_current_angle();
        double sec_cur_angle = sec_motor.get_current_angle();

        double main_target, sec_target;

        end_effector_control(speed_x, speed_y, main_cur_angle, sec_cur_angle, main_target, sec_target);

        main_motor.set_target_angle(main_target);
        sec_motor.set_target_angle(sec_target);

        screen.get_screen_handle()->setTextCursor(0, 0);
        screen.get_screen_handle()->printf("M_a: %.6f    \n", main_cur_angle);
        screen.get_screen_handle()->printf("S_a: %.6f    \n", sec_cur_angle);
        screen.get_screen_handle()->printf("Vx:  %.6f \n", main_target);
        screen.get_screen_handle()->printf("Vy:  %.6f ", sec_target);
        screen.display();
    }
}

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;
        led_blue = 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 demo:
            do_state_demo();
            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);

    // Set the maximum speed for both motors.
    main_motor.set_max_speed(0.5);
    sec_motor.set_max_speed(0.5);

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

    emg_1.start(emg_period);
    emg_2.start(emg_period);

    // Start the button polling ticker.
    button_ticker.attach(&poll_buttons, button_poll_interval);

    while (true) {
        main_loop();

        wait(main_loop_wait_time);
    }
}