File content as of revision 33:1da600f06862:

#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.1416;
const float l = 0.535;
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;
volatile float  theta_ref2;
float Ts    = 0.01f;
float Kp;
float Ki;
float Kd;
float theta_1 = (40.0f*pi)/180.0f;
float theta_2 = (175.0f*pi)/180.0f;
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;
    }
}

void Controller()
{
    float K     = 1;
    float ti    = 0.1;
    float td    = 10;
    Kp    = K*(1+td/ti);
    Ki    = K/ti;
    Kd    = K*td;
    
    theta_error1 = theta_ref1-theta_1;
    theta_error2 = theta_ref2-theta_2;
    
    float error_integral1 = 0;
    float error_integral2 = 0;
    float error_prev1 = 0;
    float error_prev2 = 0;
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);

    // Proportional part:
    float torque_p1 = Kp * theta_error1;
    float torque_p2 = Kp * theta_error2;
    
    // Integral part:
    error_integral1 = error_integral1 + theta_error1 * Ts;
    error_integral2 = error_integral2 + theta_error2 * Ts;
    float torque_i1 = Ki * error_integral1;
    float torque_i2 = Ki * error_integral2;

    // Derivative part:
    float error_derivative1 = (theta_error1 - error_prev1)/Ts;
    float error_derivative2 = (theta_error2 - error_prev2)/Ts;
    float filtered_error_derivative1 = LowPassFilter.step(error_derivative1);
    float filtered_error_derivative2 = LowPassFilter.step(error_derivative2);
    float torque_d1 = Kd * filtered_error_derivative1;
    float torque_d2 = Kd * filtered_error_derivative2;
    error_prev1 = theta_error1;
    error_prev2 = theta_error2;

    // Sum all parts and return it
    torque_1 = torque_p1 + torque_i1 + torque_d1;
    torque_2 = torque_p2 + torque_i2 + torque_d2;
}

void Kinematics()
{
    float DET_jacobian= 1.0f/((-l*sin(theta_1)-l*sin(theta_1+theta_2))*(l*cos(theta_1+theta_2))-(-l*sin(theta_1+theta_2))*(l*cos(theta_1)+l*cos(theta_1+theta_2)));  
    float thetav_1=DET_jacobian*l*cos(theta_1+theta_2)*EMGx_velocity + DET_jacobian*l*sin(theta_1+theta_2)*EMGy_velocity;
    float thetav_2= DET_jacobian*-l*cos(theta_1)-l*cos(theta_1+theta_2)*EMGx_velocity + DET_jacobian*-l*sin(theta_1)-l*sin(theta_1+theta_2)*EMGy_velocity;
    theta_ref1=theta_1+thetav_1*Ts;
    theta_ref2=theta_2+thetav_2*Ts;
    x=cos(theta_ref1)*l+cos(theta_ref1+theta_ref2)*l;
    y=sin(theta_ref1)*l+sin(theta_ref1+theta_ref2)*l;  
    if (sqrt(pow(x,2)+pow(y,2))>1.0f) {
        theta_ref1 = theta_1;
        theta_ref2 = theta_2;
    }
}
void CalculateDirectionMotor()
{
    Kinematics();
    Controller();
    direction_motor_1 = torque_1 <= 0.0f;
    direction_motor_2 = torque_2 <= 0.0f;
}

void ReadEncoder()
{
    theta_1 = (((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 = (((2.0f*pi)/64.0f)*(float)encoder_2.getPulses())/131.25f;
}

void MotorControl()
{
    ReadEncoder();
    CalculateDirectionMotor();
    PWM_motor_1.write(fabs(torque_1)/(2.0f*pi));
    PWM_motor_2.write(fabs(torque_2)/(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, EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl));
    scope.set(1, EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br));
    scope.set(2, EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg));
    scope.set(3, emgFiltered_bl);
    scope.set(4, emgFiltered_br);
    scope.set(5, emgFiltered_leg);
    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()
{
    // Do demo mode stuff
    treshold_bl = 1.1f;
    treshold_br = 1.1f;
    treshold_leg = 1.1f;
    if ((pressed_1) || (pressed_2)) {
        CurrentState = vertical_movement;
        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
    if (emg_trigger(treshold_bl,EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl))) {
        EMGy_velocity = -0.02f;
    }
    else if (emg_trigger(treshold_br,EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br))) {
        EMGy_velocity = 0.02f;
    }    
    else {
        EMGy_velocity = 0.0f;
    }
    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;
    }*/
      m++;
    if (m<5) {
        EMGy_velocity = -0.02f;
        pc.printf("beweging %f \n\r", EMGy_velocity);
        
    }
    else if (m>=5 && m<=10) {
        EMGy_velocity = 0.02f;
        pc.printf("beweging %f \n\r", EMGy_velocity);
    }    
    
    else {
        EMGy_velocity = 0.0f;
        //pc.printf("beweging %f \n\r", EMGy_velocity);
    }

    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
    
    if ((emg_trigger(treshold_bl,EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl))) || (get_command_a())){
        EMGx_velocity = -0.02f;
        pc.printf("beweging %f \n\r", EMGx_velocity);
    }
    if (emg_trigger(treshold_br,EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br)) || (get_command_d())) {
        EMGx_velocity = 0.02f;
        pc.printf("beweging %f \n\r", EMGx_velocity);
    }    
    if ((!(emg_trigger(treshold_bl,EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl))) && !(emg_trigger(treshold_br,EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br)))) && (!get_command_a() && !get_command_d())) {
        EMGx_velocity = 0.0f;
        pc.printf("beweging %f \n\r", EMGx_velocity);
    }
    if ((pressed_1) || (pressed_2) || (emg_switch(treshold_leg,EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg))) || (get_command_s())) { // EMG gebaseerde threshold aanmaken
        CurrentState = vertical_movement;
        StateChanged = true;
    }
    
    /*
    if (beweging == 'a') {
        EMGx_velocity = -0.02f;
        pc.printf(" you pressed %c \n\r" , beweging);
    
    }
    if (beweging == 'd') {
        EMGx_velocity = 0.02f;
        pc.printf(" you pressed %c \n\r" , beweging);
    }    
    else {
        EMGx_velocity = 0.0f;
    }

    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(10);
    PWM_motor_2.period_ms(10);
    motor_control.attach(&MotorControl, Ts);
    write_scope.attach(&WriteScope, 0.01);
    while(true) {
        StateMachine();
    }
}