File content as of revision 36:66f500e387c4:

#include "QEI.h"
#include "mbed.h"
#include "BiQuad.h"
#include "FastPWM.h"
#include "HIDScope.h"
#include "MODSERIAL.h"

// Pins
MODSERIAL pc(USBTX, USBRX);

QEI encoder_1(D10,D11,NC,8400,QEI::X4_ENCODING);
QEI encoder_2(D12,D13,NC,8400,QEI::X4_ENCODING);

FastPWM     PWM_motor_1(D6);
FastPWM     PWM_motor_2(D5);

DigitalOut  direction_motor_1(D7);
DigitalOut  direction_motor_2(D4);

DigitalOut  led_red(LED1);
DigitalOut  led_green(LED2);
DigitalOut  led_blue(LED3);

AnalogIn    emg_bl(A0);
AnalogIn    emg_br(A1);
AnalogIn    emg_leg(A2);

InterruptIn button_1(SW2);
InterruptIn button_2(SW3);


// variables
int m = 0;

const float pi = 3.1416f;
const float l = 0.535f;
Ticker          TickerStateMachine;
Ticker          motor_control;
Ticker          write_scope;
Timer           sinus_time;
Timeout         rest_timeout;
Timeout         mvc_timeout;
Timeout         led_timeout;
enum    states  {start, motor_calibration, demo_mode, emg_calibration, vertical_movement, horizontal_movement};
states  CurrentState = start;
bool    StateChanged = true;
enum    substates {rest_biceps_left, mvc_biceps_left, rest_biceps_right, mvc_biceps_right, rest_biceps_leg, mvc_biceps_leg};
substates CurrentSubstate = rest_biceps_left;
bool    SubstateChanged = true;
volatile bool    pressed_1    = false;
volatile bool    pressed_2    = false;
HIDScope scope(6);

volatile float  theta_ref1 = 0.5f*pi;
volatile float  theta_ref2 = 3.0f*pi/4.0f;
float Ts    = 0.1f;
float Kp0;
float Ki0;
float Kd0;
float Kp1;
float Ki1;
float Kd1;
float theta_1;
float theta_2;
float theta_error1;
float theta_error2;
float torque_1;
float torque_2;
float x;
float y;
volatile float EMGx_velocity = 0.0f;
volatile float EMGy_velocity = 0.0f;

BiQuad Lowpass_bl ( 1.55148e-04, 3.10297e-04, 1.55148e-04, -1.96446e+00, 9.65081e-01 );
BiQuad Highpass_bl ( 9.40809e-01, -9.40809e-01, 0.00000e+00, -8.81619e-01, 0.00000e+00);
BiQuad notch_bl (9.98432e-01, -1.89913e+00, 9.98432e-01, -1.89913e+00, 9.96886e-01);
BiQuad Lowpass_br ( 1.55148e-04, 3.10297e-04, 1.55148e-04, -1.96446e+00, 9.65081e-01 );
BiQuad Highpass_br ( 9.40809e-01, -9.40809e-01, 0.00000e+00, -8.81619e-01, 0.00000e+00);
BiQuad notch_br (9.98432e-01, -1.89913e+00, 9.98432e-01, -1.89913e+00, 9.96886e-01);
BiQuad Lowpass_leg ( 1.55148e-04, 3.10297e-04, 1.55148e-04, -1.96446e+00, 9.65081e-01 );
BiQuad Highpass_leg ( 9.40809e-01, -9.40809e-01, 0.00000e+00, -8.81619e-01, 0.00000e+00);
BiQuad notch_leg (9.98432e-01, -1.89913e+00, 9.98432e-01, -1.89913e+00, 9.96886e-01);

int n = 0;

float emgFiltered_bl;
float emgFiltered_br;
float emgFiltered_leg;
float emg;
float xmvc_value = 1e-11;

float sum = 0;
float xrest_value;
float rest_value_bl;
float rest_value_br;
float rest_value_leg;

float mvc_value_bl;
float mvc_value_br;
float mvc_value_leg;

float treshold_bl = 0.5;
float treshold_br = 0.5;
float treshold_leg = 0.5;

bool previous_value_emg_leg;
bool current_value_emg_leg;

volatile char command;

// functies
void ledred()
{
    led_red = 0;
    led_green = 1;
    led_blue = 1;
}
void ledgreen()
{
    led_green=0;
    led_blue=1;
    led_red=1;
}
void ledblue()
{
    led_green=1;
    led_blue=0;
    led_red=1;
}
void ledyellow()
{
    led_green=0;
    led_blue=1;
    led_red=0;
}
void ledmagenta()
{
    led_green=1;
    led_blue=0;
    led_red=0;
}
void ledcyan()
{
    led_green=0;
    led_blue=0;
    led_red=1;
}
void ledwhite()
{
    led_green=0;
    led_blue=0;
    led_red=0;
}
void ledoff()
{
    led_green=1;
    led_blue=1;
    led_red=1;
}

bool get_command_a() {
    command = pc.getcNb();
    if (command == 'a') {
        pc.printf("a is ingedrukt! \n\r");   
        return true;
    } else {
    return false;
    }
}

bool get_command_d () {
    command = pc.getcNb();
    if (command == 'd') {
        pc.printf("d is ingedrukt! \n\r");   
        return true;
    } else {
    return false;
    }
}

bool get_command_s () {
    command = pc.getcNb();
    if (command == 's') {
        pc.printf("s is ingedrukt! \n\r");   
        return true;
    } else {
    return false;
    }
}

float CalculateError(float theta_reference,float theta)
{
    float theta_error = theta_reference-theta;
    return theta_error;
}

float Controller(float theta_error, bool motor)
{
    if (motor == false) {
        float K0     = 75.0f;
        float ti0    = 5.0f;
        float td0    = 0.1f;
        Kp0    = K0*(1+td0/ti0);
        Ki0    = K0/ti0;
        Kd0    = K0*td0;
        
        static float error_integral0 = 0;
        static float error_prev0 = 0;
        static BiQuad LowPassFilter0(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
    
        // Proportional part:
        float torque_p0 = Kp0 * theta_error;
    
        // Integral part:
        error_integral0 = error_integral0 + theta_error * Ts;
        float torque_i0 = Ki0 * error_integral0;
    
        // Derivative part:
        float error_derivative0 = (theta_error - error_prev0)/Ts;
        float filtered_error_derivative0 = LowPassFilter0.step(error_derivative0);
        float torque_d0 = Kd0 * filtered_error_derivative0;
        error_prev0 = theta_error;
    
        // Sum all parts and return it
        float torque0 = torque_p0 + torque_i0 + torque_d0;
        return torque0;
    } else {
        float K1     = 75.0f;
        float ti1   = 1.0f;
        float td1    = 0.01f;
        Kp1    = K1*(1+td1/ti1);
        Ki1    = K1/ti1;
        Kd1    = K1*td1;
        
        static float error_integral1 = 0;
        static float error_prev1 = 0;
        static BiQuad LowPassFilter1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
    
        // Proportional part:
        float torque_p1 = Kp1 * theta_error;
    
        // Integral part:
        error_integral1 = error_integral1 + theta_error * Ts;
        float torque_i1 = Ki1 * error_integral1;
    
        // Derivative part:
        float error_derivative1 = (theta_error - error_prev1)/Ts;
        float filtered_error_derivative1 = LowPassFilter1.step(error_derivative1);
        float torque_d1 = Kd1 * filtered_error_derivative1;
        error_prev1 = theta_error;
    
        // Sum all parts and return it
        float torque1 = torque_p1 + torque_i1 + torque_d1;
        return torque1;
    }
}

void Kinematics()
    {    
    EMGx_velocity = 0.1f;  
    EMGy_velocity = 0.0f; 
    float thetav_1= -(EMGx_velocity*cos(theta_1 + theta_2))/(l*cos(theta_1 + theta_2)*sin(theta_1) - l*sin(theta_1 + theta_2)*cos(theta_1)) - (EMGy_velocity*sin(theta_1 + theta_2))/(l*cos(theta_1 + theta_2)*sin(theta_1) - l*sin(theta_1 + theta_2)*cos(theta_1));
    float thetav_2= (EMGx_velocity*(cos(theta_1 + theta_2) + cos(theta_1)))/(l*cos(theta_1 + theta_2)*sin(theta_1) - l*sin(theta_1 + theta_2)*cos(theta_1)) + (EMGy_velocity*(sin(theta_1 + theta_2) + sin(theta_1)))/(l*cos(theta_1 + theta_2)*sin(theta_1) - l*sin(theta_1 + theta_2)*cos(theta_1));
    theta_ref1 = theta_ref1 + thetav_1*Ts;
    theta_ref2 = theta_ref2 + thetav_2*Ts;
    x = cos(theta_ref1)*l+cos(theta_ref1+theta_ref2)*l;
    y = l*(sin(theta_ref1)+sin(theta_ref1+theta_ref2));  
    if (sqrt(pow(x,2)+pow(y,2))>1.0f) {
        pc.printf("Limit reached!\n\r");
        theta_ref1 = theta_1;
        theta_ref2 = theta_2;
    }
}
void CalculateDirectionMotor()
{
    Kinematics();
    direction_motor_1 = Controller(CalculateError(theta_ref1,theta_1),0) >= 0.0f;
    direction_motor_2 = Controller(CalculateError(theta_ref2,theta_2),1) >= 0.0f;
}

void ReadEncoder()
{
    theta_1 = (0.5f*pi-(((2.0f*pi)/64.0f)*(float)encoder_1.getPulses())/131.25f); // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360.
    theta_2 = ((-3.0f*pi/4.0f)+(((2.0f*pi)/64.0f)*(float)encoder_2.getPulses())/131.25f);
}

void MotorControl()
{
    ReadEncoder();
    CalculateDirectionMotor();
    PWM_motor_1.write(fabs(Controller(CalculateError(theta_ref1,theta_1),0))/(2.0f*pi));
    PWM_motor_2.write(fabs(Controller(CalculateError(theta_ref2,theta_2),1))/(2.0f*pi));
}

void go_next_1()
{
    pressed_1 = !pressed_1;
}

void go_next_2()
{
    pressed_2 = !pressed_2;
}

bool emg_switch(float treshold, float emg_input) {
    if(emg_input > treshold){
        current_value_emg_leg = true;
    } else {
        current_value_emg_leg = false;
    }
    if(current_value_emg_leg == true && previous_value_emg_leg == false) {
        previous_value_emg_leg = current_value_emg_leg;
        return true;
    } else {
        previous_value_emg_leg = current_value_emg_leg;
        return false;
    }
}

bool emg_trigger(float treshold, float emg_input) {
    if(emg_input > treshold) {
        return true;
    } else {
        return false;
    }
}

float EmgCalibration(float emgFiltered, float mvc_value, float rest_value)
{
    float emgCalibrated;
    if (emgFiltered <= rest_value) {
        return 0.0f;
        //emgCalibrated = 0;
    }
    
    if (emgFiltered >= mvc_value) {
        return 1.1f;
        //emgCalibrated = 1;
    } else {
        emgCalibrated = (emgFiltered-rest_value)/(mvc_value-rest_value);
    }
    return emgCalibrated;
}

void emgsample()
{
    emgFiltered_bl = Highpass_bl.step(emg_bl.read());
    emgFiltered_bl = notch_bl.step(emgFiltered_bl);
    emgFiltered_bl = fabs(emgFiltered_bl);
    emgFiltered_bl = Lowpass_bl.step(emgFiltered_bl);

    emgFiltered_br = Highpass_br.step(emg_br.read());
    emgFiltered_br = notch_br.step(emgFiltered_br);
    emgFiltered_br = fabs(emgFiltered_br);
    emgFiltered_br = Lowpass_br.step(emgFiltered_br);

    emgFiltered_leg = Highpass_leg.step(emg_leg.read());
    emgFiltered_leg = notch_leg.step(emgFiltered_leg);
    emgFiltered_leg = fabs(emgFiltered_leg);
    emgFiltered_leg = Lowpass_leg.step(emgFiltered_leg);
}

void rest()
{
    if (CurrentSubstate == rest_biceps_left) {
        emg = emgFiltered_bl;
        //pc.printf("emg: %f \n\r",emgFiltered_bl);
    }
    if (CurrentSubstate == rest_biceps_right) {
        emg = emgFiltered_br;
    }
    if (CurrentSubstate == rest_biceps_leg) {
        emg = emgFiltered_leg;
    }
    if (n < 500) {
        ledred();
        sum = sum + emg;
        //pc.printf("sum: %f \n\r",sum);
        n++;
        rest_timeout.attach(rest,0.001f);
    }
    if (n == 500) {
        sum = sum + emg;
        //pc.printf("sum: %f \n\r",sum);
        n++;
        xrest_value = float (sum/n);
        if (CurrentSubstate == rest_biceps_left) {
            rest_value_bl = xrest_value;
            pc.printf("rest_value_bl %f \n\r", rest_value_bl);
            CurrentSubstate = mvc_biceps_left;
            SubstateChanged = true;
            ledblue();
            
        }
        if (CurrentSubstate == rest_biceps_right) {
            rest_value_br = xrest_value;
            pc.printf("rest_value_br %f \n\r", rest_value_br);
            CurrentSubstate = mvc_biceps_right;
            SubstateChanged = true;
            ledmagenta();
        }
        if (CurrentSubstate == rest_biceps_leg) {
            rest_value_leg = xrest_value;
            pc.printf("rest_value_leg %f \n\r", rest_value_leg);
            pc.printf("rest_value_bl %f \n\r", rest_value_bl);
            CurrentSubstate = mvc_biceps_leg;
            SubstateChanged = true;
            ledwhite();
        }
    }
}

void mvc()
{
    if (CurrentSubstate == mvc_biceps_left) {
        emg = emgFiltered_bl;
    }
    if (CurrentSubstate == mvc_biceps_right) {
        emg = emgFiltered_br;
    }
    if (CurrentSubstate == mvc_biceps_leg) {
        emg = emgFiltered_leg;
    }
    if (emg >= xmvc_value) {
        xmvc_value = emg;
    }
    n++;
    if (n < 1000) {
        mvc_timeout.attach(mvc,0.001f);
        ledred();
    }
    if (n == 1000) {
        if (CurrentSubstate == mvc_biceps_left) {
            mvc_value_bl = xmvc_value;
            pc.printf("mvc_value_bl %f \n\r", mvc_value_bl);
            CurrentSubstate = rest_biceps_right;
            SubstateChanged = true;
            ledyellow();
        }
        if (CurrentSubstate == mvc_biceps_right) {
            mvc_value_br = xmvc_value;
            pc.printf("mvc_value_br %f \n\r", mvc_value_br);
            CurrentSubstate = rest_biceps_leg;
            SubstateChanged = true;
            ledcyan();
        }
        if (CurrentSubstate == mvc_biceps_leg) {
            mvc_value_leg = xmvc_value;
            pc.printf("mvc_value_leg %f \n\r", mvc_value_leg);
            CurrentState = vertical_movement;
            StateChanged = true;
            ledoff();
        }
        xmvc_value = 1e-11;
    }
}

void WriteScope()
{
    emgsample();
    scope.set(0, theta_ref1);
    scope.set(1, theta_1);
    scope.set(2, CalculateError(theta_ref1,theta_1));
    scope.set(3, theta_ref2);
    scope.set(4, theta_2);
    scope.set(5, CalculateError(theta_ref2,theta_2));
    scope.send();
}

void SubstateTransition()
{
    if (SubstateChanged == true) {
        SubstateChanged = false;
        pressed_1 = false;
        pressed_2 = false;
        if (CurrentSubstate == rest_biceps_left) {
            ledgreen();
            pc.printf("groen \n\r");
            pc.printf("Initiating rest_biceps_left\n\r");
        }
        if (CurrentSubstate == mvc_biceps_left) {
            //ledblue();
            pc.printf("Initiating mvc_biceps_left\n\r");
        }
        if (CurrentSubstate == rest_biceps_right) {
            //ledyellow();
            pc.printf("Initiating rest_biceps_right\n\r");
        }
        if (CurrentSubstate == mvc_biceps_right) {
            //ledmagenta();
            pc.printf("Initiating mvc_biceps_right\n\r");
        }
        if (CurrentSubstate == rest_biceps_leg) {
            //ledcyan();
            pc.printf("Initiating rest_biceps_leg\n\r");
        }
        if (CurrentSubstate == mvc_biceps_leg) {
            //ledwhite();
            pc.printf("Initiating mvc_biceps_leg\n\r");
        }
    }
}

void while_start()
{
    // Do startup stuff
    CurrentState = motor_calibration;
    StateChanged = true;
}

void while_motor_calibration()
{
    // Do motor calibration stuff
    if (pressed_1) { // bool aanmaken voor demo (switch oid aanmaken)
        CurrentState = demo_mode;
        StateChanged = true;
    }
    if (pressed_2) {  // bool aanmaken voor EMG (switch oid aanmaken)
        CurrentState = emg_calibration;
        StateChanged = true;
    }
}

void while_demo_mode()
{
    EMGx_velocity = 0.1f;
    EMGy_velocity = 0.0f;
    ReadEncoder();
    Kinematics();
    
    pc.printf("X: %f    Y: %f   1: %f    2: %f  \n\r",x,y,theta_1,theta_2);
    /*
    m++;
    
    if (m<5) {
        EMGy_velocity = 0.1f;
        EMGx_velocity = 0.0f;
        pc.printf("beweging in y richting %f \n\r", EMGy_velocity);
        pc.printf("thetav_1 %f \n\r", theta_ref1);
    }
    if (m>=5 && m<10) {
        EMGy_velocity = 0.0f;
        EMGx_velocity = 0.1f;
        pc.printf("beweging in x richting %f \n\r", EMGx_velocity);
        pc.printf("thetav_1 %f \n\r", theta_ref1);
    }
    if (m>=10 && m<15) {
        EMGy_velocity = -0.1f;
        EMGx_velocity = 0.0f;
        pc.printf("beweging in y richting %f \n\r", EMGy_velocity);
    }
    if (m>=15 && m<20) {
        EMGy_velocity = 0.0f;
        EMGx_velocity = -0.1f;
        pc.printf("beweging in x richting %f \n\r", EMGx_velocity);
    }
    if (pressed_1) {
        m=0;
        CurrentState = demo_mode;
        StateChanged = true;
    }
    if ((pressed_2)) {
        CurrentState = emg_calibration;
        StateChanged = true;
    }
    */
}

void while_emg_calibration()
{
    // Do emg calibration stuff
    switch (CurrentSubstate) {
        case rest_biceps_left:
            SubstateTransition();
            if ((pressed_1) || (pressed_2)) {
                pressed_1 = false;
                pressed_2 = false;
                n = 0;
                sum = 0;
                rest();
            }
            break;
        case mvc_biceps_left:
            SubstateTransition();
            if ((pressed_1) || (pressed_2)) {
                pressed_1 = false;
                pressed_2 = false;
                n = 0;
                mvc();
            }
            break;
        case rest_biceps_right:
            SubstateTransition();
            if ((pressed_1) || (pressed_2)) {
                pressed_1 = false;
                pressed_2 = false;
                n = 0;
                sum = 0;
                rest();
            }
            break;
        case mvc_biceps_right:
            SubstateTransition();
            if ((pressed_1) || (pressed_2)) {
                pressed_1 = false;
                pressed_2 = false;
                n = 0;
                mvc();
            }
            break;
        case rest_biceps_leg:
            SubstateTransition();
            if ((pressed_1) || (pressed_2)) {
                pressed_1 = false;
                pressed_2 = false;
                n = 0;
                sum = 0;
                rest();
            }
            break;
        case mvc_biceps_leg:
            SubstateTransition();
            if ((pressed_1) || (pressed_2)) {
                pressed_1 = false;
                pressed_2 = false;
                n = 0;
                mvc();
            }
            break;
        default:
            pc.printf("Unknown or unimplemented state reached!\n\r");
    }
}

void while_vertical_movement()
{
    // Do vertical movement stuff
    MotorControl();
    if ((pressed_1) || (pressed_2) || (emg_switch(treshold_leg,EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg)))) { // EMG gebaseerde threshold aanmaken
        CurrentState = horizontal_movement;
        StateChanged = true;  
    }
}


void while_horizontal_movement()
{
    // Do horizontal movement stuff
    MotorControl();
    if ((pressed_1) || (pressed_2) || (emg_switch(treshold_leg,EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg)))) { // EMG gebaseerde threshold aanmaken
        CurrentState = vertical_movement;
        StateChanged = true;
    }
}    


void StateTransition()
{
    if (StateChanged) {
        pressed_1 = false;
        pressed_2 = false;
        if (CurrentState == start) {
            pc.printf("Initiating start.\n\r");
        }
        if (CurrentState == motor_calibration) {
            pc.printf("Initiating motor_calibration.\n\r");
        }
        if (CurrentState == demo_mode) {
            pc.printf("Initiating demo_mode.\n\r");
        }
        if (CurrentState == emg_calibration) {
            pc.printf("Initiating emg_calibration.\n\r");
        }
        if (CurrentState == vertical_movement) {
            pc.printf("Initiating vertical_movement.\n\r");
        }
        if (CurrentState == horizontal_movement) {
            pc.printf("Initiating horizontal_movement.\n\r");
        }
        StateChanged = false;
    }
}

void StateMachine()
{
    switch(CurrentState) {
        case start:
            StateTransition();
            while_start();
            break;
        case motor_calibration:
            StateTransition();
            while_motor_calibration();
            break;
        case demo_mode:
            StateTransition();
            while_demo_mode();
            break;
        case emg_calibration:
            StateTransition();
            while_emg_calibration();
            break;
        case vertical_movement:
            StateTransition();
            while_vertical_movement();
            break;
        case horizontal_movement:
            StateTransition();
            while_horizontal_movement();
            break;
        default:
            pc.printf("Unknown or unimplemented state reached!\n\r");
    }
}

// main
int main()
{
    pc.baud(115200);
    pc.printf("Hello World!\n\r");
    ledoff();
    button_1.fall(go_next_1);
    button_2.fall(go_next_2);
    sinus_time.start();
    PWM_motor_1.period_ms(1.0f/16.0f);
    PWM_motor_2.period_ms(1.0f/16.0f);
    //motor_control.attach(&MotorControl, Ts);
    write_scope.attach(&WriteScope, 0.01);
    TickerStateMachine.attach(&StateMachine,Ts);
    while(true) {
        return 0;
    }
}