File content as of revision 24:7cad312a1e38:

/*
To-do:
    Add reference generator
    fully implement schmitt trigger 
    Homing 
    Turning the magnet on/off
    Inverse kinematics
    Gravity compensation
    PID values
    General program layout
    better names for EMG input
*/

#include "mbed.h"
#include "MODSERIAL.h"
#include "FastPWM.h"
#include "QEI.h"
#include "HIDScope.h"
#include "BiQuad.h"
#define PI 3.14159265

Serial pc(USBTX, USBRX);            //connect to pc
HIDScope scope(4);                  //HIDScope instance
DigitalOut motor1_direction(D4);    //rotation motor 1 on shield (always D6)
FastPWM motor1_pwm(D5);             //pwm 1 on shield (always D7)
DigitalOut motor2_direction(D7);    //rotation motor 2 on shield (always D4)
FastPWM motor2_pwm(D6);             //pwm 2 on shield (always D5)
Ticker loop_ticker;                 //used in main()
Ticker scope_ticker;
InterruptIn but1(SW2);              //debounced button on biorobotics shield
InterruptIn but2(D9);               //debounced button on biorobotics shield
AnalogIn EMG1(A0);
AnalogIn EMG2(A1);
AnalogIn EMG3(A4);
AnalogIn EMG4(A5);
DigitalOut led_r(LED1);
DigitalOut led_g(LED2);
DigitalOut led_b(LED3);

void check_failure();
int schmitt_trigger(float i);

QEI enc1 (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder 1 gebruiken
QEI enc2 (D1,  D2,  NC, 8400, QEI::X4_ENCODING); //encoder 2 gebruiken

BiQuad bq1 (0.881889334678067,    -1.76377866935613,    0.8818893346780671,   -1.77069673005903, 0.797707797506027);
BiQuad bq2 (8.05339325492925e-06, 1.61067865098585e-05, 8.05339325492925e-06, -1.99254216118629, 0.992574747774901);

//variables
enum States {s_idle, s_cali_EMG, s_cali_enc, s_moving_magnet_off, s_moving_magnet_on, s_homing, s_failure};
States state;                       //using the States enum
struct actuator_state {
    float duty_cycle1;              //pwm of 1st motor
    float duty_cycle2;              //pwm of 2nd motor
    int dir1;                       //direction of 1st motor
    int dir2;                       //direction of 2nd motor
    bool magnet;                    //state of the magnet
} actuator;

struct EMG_params {
    float max;  //params of the emg, tbd during calibration
    float min;
} EMG_values1, EMG_values2;

struct PID {
    float P;
    float I;
    float D;
    float I_counter;
};
PID PID1;
PID PID2;

float dt = 0.001;
float theta;
float L;
int enc1_zero = 0;//the zero position of the encoders, to be determined from the
int enc2_zero = 0;//encoder calibration
float EMG1_filtered; // x
float EMG2_filtered; // x
float EMG3_filtered; // y
float EMG4_filtered; // y
int enc1_value;
int enc2_value;
float error1 = 0.0;
float error2 = 0.0;
float last_error1 = 0.0;
float last_error2 = 0.0;
float action1;
float action2;
bool state_changed = false; //used to see if the state is "starting"
volatile bool but1_pressed = false;
volatile bool but2_pressed = false;
volatile bool failure_occurred = false;
bool EMG_has_been_calibrated;
bool button1_pressed;
bool button2_pressed;
float filter_value1;
float filter_value2;
int past_speed1 = 0;
int past_speed2 = 0;

void do_nothing()

/*
    Idle state. Used in the beginning, before the calibration states.
*/
{
    if (button1_pressed) {
        state_changed = true;
        state = s_cali_EMG;
        button1_pressed = false;
    }
}

void failure()
/*
    Failure mode. This should execute when button 2 is pressed during operation.
*/
{
    if (state_changed) {
        pc.printf("Something went wrong!\r\n");
        state_changed = false;
    }
}

const int EMG_cali_amount1 = 1000;
float past_EMG_values1[EMG_cali_amount1];
int past_EMG_count1 = 0;

const int EMG_cali_amount2 = 1000;
float past_EMG_values2[EMG_cali_amount2];
int past_EMG_count2 = 0;

void cali_EMG()
/*
    Calibration of the EMG. Values determined during calibration should be
    added to the EMG_params instance.
*/
{
    if (state_changed) {
        pc.printf("Started EMG calibration\r\nTime is %i\r\n", us_ticker_read());
        state_changed = false;
    }
    if (past_EMG_count1 < EMG_cali_amount1) {
        past_EMG_values1[past_EMG_count1] = EMG1_filtered;
        past_EMG_count1++;
    }
    if (past_EMG_count2 < EMG_cali_amount2) {
        past_EMG_values2[past_EMG_count2] = EMG3_filtered;
        past_EMG_count2++;
    }
    
    else { //calibration has concluded
        pc.printf("Calibration concluded.\r\nTime is %i\r\n", us_ticker_read());
        float sum1 = 0.0;
        for(int i = 0; i<EMG_cali_amount1; i++) {
            sum1 += past_EMG_values1[i];
        }
        float mean1 = sum1/(float)EMG_cali_amount1;
        EMG_values1.min = mean1;
        /*
        float sum2 = 0.0;
        for(int i = 0; i<EMG_cali_amount1; i++) {
            sum1 += past_EMG_values1[i];
        }
        float mean2 = sum2/(float)EMG_cali_amount2;
        EMG_values2.min = mean2;
        */
        //calibration done, moving to cali_enc
        pc.printf("1st Min value: %f\r\n 2nd Min value: %f\r\n", EMG_values1.min, EMG_values2.min);
        pc.printf("1st Length: %f\r\n 2nd Length: %f\r\n", (float)EMG_cali_amount1, (float)EMG_cali_amount2);
        pc.printf("Calibration of the EMG is done, 1st lower bound = %f\r\n 2nd lower bound = %f\r\n", mean1, mean2);
        EMG_has_been_calibrated = true;
        state_changed = true;
        state = s_cali_enc;
    }
}

void cali_enc()
/*
    Calibration of the encoder. The encoder should be moved to the lowest
    position for the linear stage and the horizontal postition for the
    rotating stage.
*/
{
    if (state_changed) {
        pc.printf("Started encoder calibration\r\n");
        state_changed = false;
    }
    if (button1_pressed) {
        pc.printf("Encoder has been calibrated");
        enc1_zero = enc1_value;
        enc2_zero = enc2_value;
        button1_pressed = false;
        state = s_moving_magnet_off;
        state_changed = true;

    }
}

void moving_magnet_off()
/*
    Moving with the magnet disabled. This is the part from the home position
    towards the storage of chips.
*/
{
    if (state_changed) {
        pc.printf("Moving without magnet\r\n");
        state_changed = false;
    }
}

void moving_magnet_on()
/*
    Moving with the magnet enabled. This is the part of the movement from the
    chip holder to the top of the playing board.
*/
{
    if (state_changed) {
        pc.printf("Moving with magnet\r\n");
        state_changed = false;
    }
    return;
}
void homing()
/*
    Dropping the chip and moving towards the rest position.
*/
{
    if (state_changed) {
        pc.printf("Started homing");
        state_changed = false;
    }
    return;
}

float EMG1_raw;
float EMG2_raw;
float EMG3_raw;
float EMG4_raw;

void measure_signals()
{
    //only one emg input, reference and plus
    EMG1_raw = EMG1.read();
    EMG2_raw = EMG2.read();
    EMG3_raw = EMG3.read();
    EMG4_raw = EMG4.read();
    filter_value1 = fabs(bq2.step(fabs(bq1.step(EMG1_raw - EMG2_raw))));
    filter_value2 = fabs(bq2.step(fabs(bq1.step(EMG3_raw - EMG4_raw))));
    
    if (filter_value1 > EMG_values1.max) {
        EMG_values1.max = filter_value1;
    }
    if (EMG_has_been_calibrated) {
        EMG1_filtered = (filter_value1-EMG_values1.min)/(EMG_values1.max-EMG_values1.min);
    }
    else {
        EMG1_filtered = filter_value1;
    }
    
    if (filter_value2 > EMG_values2.max) {
        EMG_values2.max = filter_value2;
    }
    if (EMG_has_been_calibrated) {
        EMG3_filtered = (filter_value2-EMG_values2.min)/(EMG_values2.max-EMG_values2.min);
    }
    else {
        EMG3_filtered = filter_value2;
    }
        
    enc1_value = enc1.getPulses();
    enc2_value = enc2.getPulses();
    enc1_value -= enc1_zero;
    enc2_value -= enc2_zero;
    theta = (float)(enc1_value)/(float)(8400*2*PI);
    L     = (float)(enc2_value)/(float)(8400*2*PI);
    

}

void motor_controller() {
    int speed1 = schmitt_trigger(EMG1_filtered);
    if (speed1 == -1) {speed1 = past_speed1;}
    past_speed1 = speed1;
    
    int speed2 = schmitt_trigger(EMG3_filtered);
    if (speed2 == -1) {speed2 = past_speed1;}
    past_speed2 = speed2;
    
    
    float error1, error2;
    //P part of the controller
    float P_action1 = PID1.P * error1;
    float P_action2 = PID2.P * error2;
    
    //I part
    PID1.I_counter += error1;
    PID2.I_counter += error2;
    float I_action1 = PID1.I_counter * PID1.I;
    float I_action2 = PID2.I_counter * PID2.I;
    
    //D part
    float velocity_estimate_1 = (error1-last_error1)/dt; //estimate of the time derivative of the error
    float velocity_estimate_2 = (error2-last_error2)/dt;
    float D_action1 = velocity_estimate_1 * PID1.D;
    float D_action2 = velocity_estimate_2 * PID2.D;
    
    action1 = P_action1 + I_action1 + D_action1;
    action2 = P_action2 + I_action2 + D_action2;
        
    last_error1 = error1;
    last_error2 = error2;
}

void output()
{
    motor1_direction = actuator.dir1;
    motor2_direction = actuator.dir2;
    motor1_pwm.write(actuator.duty_cycle1);
    motor2_pwm.write(actuator.duty_cycle2);
    
    scope.set(0, EMG1_filtered);
    scope.set(1, past_speed1);
    scope.set(2, EMG3_filtered);
    scope.set(3, past_speed2);
}

void state_machine()
{
    check_failure(); //check for an error in the last loop before state machine
    //run current state
    switch (state) {
        case s_idle:
            do_nothing();
            break;
        case s_failure:
            failure();
            break;
        case s_cali_EMG:
            cali_EMG();
            break;
        case s_cali_enc:
            cali_enc();
            break;
        case s_moving_magnet_on:
            moving_magnet_on();
            break;
        case s_moving_magnet_off:
            moving_magnet_off();
            break;
        case s_homing:
            homing();
            break;
    }
}

void main_loop()
{
    measure_signals();
    state_machine();
    motor_controller();
    output();
}

//Helper functions, not directly called by the main_loop functions or
//state machines
void check_failure()
{
    //state = s_failure;
    //state_changed = true;
}

void but1_interrupt()
{
    if(but2.read()) {//both buttons are pressed
        failure_occurred = true;
    }
    button1_pressed = true;
    pc.printf("Button 1 pressed \n\r");
}

void but2_interrupt()
{
    if(but1.read()) {//both buttons are pressed
        failure_occurred = true;
    }
    button2_pressed = true;
    pc.printf("Button 2 pressed \n\r");
}

int schmitt_trigger(float i) 
{
    int speed;
    speed = -1; //default value, this means the state should not change
    if (i >  0.000 && i < 0.125) {speed = 0; led_r = 0; led_g = 1; led_b = 1;}
    if (i >  0.250 && i < 0.375) {speed = 1; led_g = 0; led_r = 1; led_b = 1;}
    if (i >  0.500 && i < 1.000) {speed = 2; led_b = 0; led_r = 1; led_g = 1;}   
    return speed;
}

int main()
{
    pc.baud(115200);
    pc.printf("Executing main()... \r\n");
    state = s_idle;

    motor2_pwm.period(1/160000);   // 1/frequency van waarop hij draait
    motor1_pwm.period(1/160000);   // 1/frequency van waarop hij draait

    actuator.dir1 = 0;
    actuator.dir2 = 0;

    actuator.magnet = false;
    EMG_values1.max = 0.01;
    EMG_values2.max = 0.01;

    but1.fall(&but1_interrupt);
    but2.fall(&but2_interrupt);
    scope_ticker.attach(&scope, &HIDScope::send, 0.02);
    loop_ticker.attach(&main_loop, dt); //main loop at 1kHz
    pc.printf("Main_loop is running\n\r");
    while (true) {
        wait(0.1f);
    }
}