File content as of revision 25:e1577c9e8c7e:

/*
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 motor0_direction(D4);    //rotation motor 1 on shield (always D6)
FastPWM motor0_pwm(D5);             //pwm 1 on shield (always D7)
DigitalOut motor1_direction(D7);    //rotation motor 2 on shield (always D4)
FastPWM motor1_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_sig(A0);
AnalogIn EMG1_ref(A1);
AnalogIn EMG2_sig(A2);
AnalogIn EMG2_ref(A3);

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_1 (0.881889334678067,    -1.76377866935613,    0.8818893346780671,   -1.77069673005903, 0.797707797506027);
BiQuad bq1_2 (8.05339325492925e-06, 1.61067865098585e-05, 8.05339325492925e-06, -1.99254216118629, 0.992574747774901);
BiQuad bq2_1 (0.881889334678067,    -1.76377866935613,    0.8818893346780671,   -1.77069673005903, 0.797707797506027);
BiQuad bq2_2 (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_cycle[2];            //pwm of 1st motor
    int direction[2];               //direction of 1st motor
    bool magnet;                    //state of the magnet
} actuator;

struct EMG_params {
    float max[2];  //params of the emg, tbd during calibration
    float min[2];
} EMG;

struct PID_struct {
    float P[2];
    float I[2];
    float D[2];
    float I_counter[2];
} PID;

float dt = 0.001;
float theta[2]; //theta0 = rot, theta1 = lin
int enc_zero[2] = {0, 0};//the zero position of the encoders, to be determined from the encoder calibration
float EMG_filtered[2];
int enc_value[2];
float current_error[2] = {0.0, 0.0};
float last_error[2] = {0.0, 0.0};
float action[2];
bool state_changed = false; //used to see if the state is "starting"
volatile bool button1_pressed = false;
volatile bool button2_pressed = false;
volatile bool failure_occurred = false;
bool EMG_has_been_calibrated;
float filter_value[2];
int speed[2];
int past_speed[2] = {0, 0};
float desired_velocity[2];
float theta_desired[2];

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_amount = 1000;
float past_EMG_values[2][EMG_cali_amount];
int past_EMG_count = 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_count < EMG_cali_amount) {
        past_EMG_values[0][past_EMG_count] = EMG_filtered[0];
        past_EMG_values[1][past_EMG_count] = EMG_filtered[1];
        past_EMG_count++;
    }
    else { //calibration has concluded
        pc.printf("Calibration concluded.\r\nTime is %i\r\n", us_ticker_read());
        float sum[2] = {0.0, 0.0};
        float mean[2] = {0.0, 0.0};
        for(int c = 0; c<2; c++){
            for(int i = 0; i<EMG_cali_amount; i++) {
                sum[c] += past_EMG_values[c][i];
            }
            mean[c] = sum[c]/(float)EMG_cali_amount;
            EMG.min[c] = mean[c];
        }
        
        //calibration done, moving to cali_enc
        pc.printf("Min values: %f %f\r\n", EMG.min[0], EMG.min[1]);
        pc.printf("Length: %f\r\n", (float)EMG_cali_amount);
        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\r\n");
        enc_zero[0] = enc_value[0];
        enc_zero[1] = enc_value[1];
        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\r\n");
        state_changed = false;
    }
    return;
}

float EMG_raw[2][2];
void measure_signals()
{
    //only one emg input, reference and plus
    EMG_raw[0][0] = EMG1_sig.read();
    EMG_raw[0][1] = EMG1_ref.read();
    EMG_raw[1][0] = EMG2_sig.read();
    EMG_raw[1][1] = EMG2_ref.read();
    filter_value[0] = fabs(bq1_2.step(fabs(bq1_1.step(EMG_raw[0][0] - EMG_raw[0][1]))));
    filter_value[1] = fabs(bq2_2.step(fabs(bq2_1.step(EMG_raw[1][0] - EMG_raw[1][1]))));
    
    enc_value[0] = enc1.getPulses();
    enc_value[1] = enc2.getPulses();
    
    for(int c = 0; c < 2; c++) {
        if (filter_value[c] > EMG.max[c]) {
            EMG.max[c] = filter_value[c];
        }
        if (EMG_has_been_calibrated) {
            EMG_filtered[c] = (filter_value[c]-EMG.min[c])/(EMG.max[c]-EMG.min[c]);
        }
        else {
            EMG_filtered[c] = filter_value[c];
        }
        enc_value[c] -= enc_zero[c];
        theta[c] = (float)(enc_value[c])/(float)(8400*2*PI);
    }
    theta[1] = theta[1]*(5.05*0.008*PI)+0.63;
    
    for(int c = 0; c<2; c++) {
        speed[c] = schmitt_trigger(EMG_filtered[c]);
        if (speed[c] == -1) {
            speed[c] = past_speed[c];
        }
        past_speed[c] = speed[c];
        if (speed[c] == 0){
            desired_velocity[c] = 0;
        }
        if (speed[c] == 1){
            desired_velocity[c] = 0.01;
        }
        if (speed[c] == 2){
            desired_velocity[c] = 0.02;
        }
    }
    theta_desired[0] = theta[0] + dt*(desired_velocity[1]*cos(theta[0])/theta[1] - desired_velocity[0]*sin(theta[0])/theta[1]);
    theta_desired[1] = theta[1] + dt*(desired_velocity[0]*cos(theta[0]) - desired_velocity[1]*sin(theta[0]));
}

void motor_controller() {
    float errors[2];
    float P_action[2];
    float I_action[2];
    float D_action[2];
    float velocity_estimate[2];
    for (int c = 0; c<2; c++) {
        
        //P action
        P_action[c] = PID.P[c] * errors[c];
           
        //I part        
        PID.I_counter[c] += errors[c];
        I_action[c] = PID.I_counter[c] * PID.I[c];
    
        //D part
        velocity_estimate[c] = (errors[c]-last_error[c])/dt; //estimate of the time derivative of the error
        D_action[c] = velocity_estimate[c] * PID.D[c];
    
        action[c] = P_action[c] + I_action[c] + D_action[c];        
        last_error[c] = errors[c];
    }
}

void output()
{
    motor0_direction = actuator.direction[0];
    motor1_direction = actuator.direction[1];
    motor0_pwm.write(actuator.duty_cycle[0]);
    motor1_pwm.write(actuator.duty_cycle[1]);
    
    scope.set(0, EMG_filtered[0]);
    scope.set(1, past_speed[0]);    
    scope.set(2, EMG_filtered[1]);
    scope.set(3, past_speed[1]);
}

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()
{
    //if (failure_occurred) {
    //    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.000f && i < 0.125f) {speed = 0;}
    if (i >  0.250f && i < 0.375f) {speed = 1;}
    if (i >  0.500f && i < 1.000f) {speed = 2;}   
    return speed;
}

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

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

    actuator.direction[0] = 0;
    actuator.direction[1] = 0;
    
    
    PID.I_counter[0] = 0.0;
    PID.I_counter[1] = 0.0;
    
    actuator.magnet = false;
    EMG.max[0] = 0.01;
    EMG.max[1] = 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);
    }
}