File content as of revision 27:d37b3a0e0f2b:

#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
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_1;
//volatile float  theta_error_1;
volatile float  theta_reference_1;
volatile float theta_2;
//volatile float  theta_error_2;
volatile float  theta_reference_2;
float Ts    = 0.01;
float Kp;
float Ki;
float Kd;


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;

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


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 K     = 1;
        float ti    = 0.1;
        float td    = 10;
        Kp    = K*(1+td/ti);
        Ki    = K/ti;
        Kd    = K*td;
    } else {
        float K     = 1;
        float ti    = 0.1;
        float td    = 10;
        Kp    = K*(1+td/ti);
        Ki    = K/ti;
        Kd    = K*td;
    }
    static float error_integral = 0;
    static float error_prev = 0;
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);

    // Proportional part:
    float torque_p = Kp * theta_error;

    // Integral part:
    error_integral = error_integral + theta_error * Ts;
    float torque_i = Ki * error_integral;

    // Derivative part:
    float error_derivative = (theta_error - error_prev)/Ts;
    float filtered_error_derivative = LowPassFilter.step(error_derivative);
    float torque_d = Kd * filtered_error_derivative;
    error_prev = theta_error;

    // Sum all parts and return it
    float torque = torque_p + torque_i + torque_d;
    return torque;
}

void CalculateDirectionMotor()
{
    direction_motor_1 = Controller(CalculateError(theta_reference_1,theta_1),0) <= 0.0f;
    direction_motor_2 = Controller(CalculateError(theta_reference_2,theta_2),1) <= 0.0f;
}

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

void MotorControl()
{
    ReadEncoder();
    theta_reference_1 = 360.0f*sin(0.1f*sinus_time.read()*2.0f*3.14f); // voor test, moet weg in eindscript
    CalculateDirectionMotor();
    PWM_motor_1.write(fabs(Controller(CalculateError(theta_reference_1,theta_1),0)/360.0f));
    PWM_motor_2.write(fabs(Controller(CalculateError(theta_reference_2,theta_2),1)/360.0f));
}

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){
        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
    if ((pressed_1) || (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
    if ((pressed_1) || (pressed_2)) { // EMG gebaseerde threshold aanmaken
        CurrentState = horizontal_movement;
        StateChanged = true;
    }
}

void while_horizontal_movement()
{
    // Do horizontal movement stuff
    if ((pressed_1) || (pressed_2)) { // 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);
    //motor_control.attach(&MotorControl, Ts);
    write_scope.attach(&WriteScope, 0.01);
    //TickerStateMachine.attach(StateMachine,1.00f);
    while(true) {
        StateMachine();
    }
}