File content as of revision 25:832b26afbe0b:

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

// Pins
MODSERIAL pc(USBTX, USBRX);
InterruptIn stepresponse(D0);

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;
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;
bool    pressed_1    = false;
bool    pressed_2    = false;
HIDScope scope(3);
QEI encoder_1(D10,D11,NC,8400,QEI::X4_ENCODING);
QEI encoder_2(D12,D13,NC,8400,QEI::X4_ENCODING);
volatile double theta_1;
//volatile float  theta_error_1;
volatile float  theta_reference_1;
volatile double theta_2;
//volatile float  theta_error_2;
volatile float  theta_reference_2;
float Ts    = 0.001;
float Kp;
float Ki;
float Kd;

BiQuadChain highnotch;
BiQuadChain low;
BiQuad Lowpass1 (3.73938e-07, 7.47876e-07, 3.73938e-07, -1.90886e+00, 9.11279e-01);
BiQuad Lowpass2 (1.00000e+00, 2.00000e+00, 1.00000e+00, -1.95979e+00, 9.62270e-01);
BiQuad Highpass (9.69531e-01, -9.69531e-01, 0.00000e+00, -9.39063e-01, 0.00000e+00 );
BiQuad Notch (9.98432e-01, -1.89913e+00, 9.98432e-01, -1.89913e+00, 9.96863e-01);
int n = 0;

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

int   muscle;
float sum = 0;
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
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*sin(0.1f*sinus_time.read()*2*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;
}

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

void emgsample()
{
    emgFiltered_bl = highnotch.step(emg_bl.read());
    emgFiltered_bl = fabs(emgFiltered_bl);
    emgFiltered_bl = low.step(emgFiltered_bl);

    emgFiltered_br = highnotch.step(emg_br.read());
    emgFiltered_br = fabs(emgFiltered_br);
    emgFiltered_br = low.step(emgFiltered_br);

    emgFiltered_leg = highnotch.step(emg_leg.read());
    emgFiltered_leg = fabs(emgFiltered_leg);
    emgFiltered_leg = low.step(emgFiltered_leg);
}

void rest()
{
    if (muscle == 0)
    {
        emg = emgFiltered_bl;
    }
    if (muscle == 2)
    {
        emg = emgFiltered_br;
    }
    if (muscle == 4)
    {
        emg = emgFiltered_leg;
    }
    if (n < 50) 
    {
        sum = sum + emg;
        n++;
        rest_timeout.attach(rest,0.01f);
    }
    if (n == 50) 
    {
        sum = sum + emg;
        n++;
        if (muscle == 0) 
        {
            rest_value_bl = float (sum/n);
            CurrentSubstate = mvc_biceps_left;
            SubstateChanged = true;
            led_red = 1;
        }
        if (muscle == 2) 
        {
            rest_value_br = float (sum/n);
            CurrentSubstate = mvc_biceps_right;
            SubstateChanged = true;
            led_red = 1;
        }
        if (muscle == 4) 
        {
            rest_value_leg = float (sum/n);
            CurrentSubstate = mvc_biceps_leg;
            SubstateChanged = true;
            led_red = 1;
        }
        n = 0;
        sum = 0;
    }
}

void mvc()
{
    if (muscle == 1)
    {
        emg = emgFiltered_bl;
    }
    if (muscle == 3)
    {
        emg = emgFiltered_br;
    }
    if (muscle == 5)
    {
        emg = emgFiltered_leg;
    }
    if (emg >= xmvc_value)
    {
        xmvc_value = emg;
    }  
    n++;
    if (n < 100) 
    {
        mvc_timeout.attach(mvc,0.01f);
    }
    if (n == 100)
    {
        n = 0;
        if (muscle == 1)
        {
            mvc_value_bl = xmvc_value;
            CurrentSubstate = rest_biceps_right;
            SubstateChanged = true;
            led_red = 1;
        }
        if (muscle == 3)
        {
            mvc_value_br = xmvc_value;
            CurrentSubstate = rest_biceps_leg;
            SubstateChanged = true;
            led_red = 1;
        }
        if (muscle == 5)
        {
            mvc_value_leg = xmvc_value;
            CurrentState = vertical_movement;
            StateChanged = true;
            led_red = 1;
        }
        xmvc_value = 1e-11;
        led_red = 1;
    }
}

void WriteScope()
{
    //scope.set(0,theta_1);
    // scope.set(1,CalculateError(theta_reference_1,theta_1));
    // scope.set(2,theta_reference_1);
    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.send();
}

void SubstateTransition()
{
    pressed_1 = false;
    pressed_2 = false;
    SubstateChanged = false;
}

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();
            muscle = 0;
            led_green = 0;
            if (pressed_1 || pressed_2) 
            {
                led_green = 1;
                led_red = 0;
                rest();
            }
            break;
        case mvc_biceps_left:
            SubstateTransition();
            muscle = 1;
            led_blue = 0;
            if (pressed_1 || pressed_2) 
            {
                led_blue = 1;
                led_red = 0;
                mvc();
            }
            break;
        case rest_biceps_right:
            SubstateTransition();
            muscle = 2;
            led_red = 0;
            led_green = 0;
            if (pressed_1 || pressed_2) 
            {
                led_green = 1;
                rest();
            }
            break;
        case mvc_biceps_right:
            SubstateTransition();
            muscle = 3;
            led_red = 0;
            led_blue = 0;
            if (pressed_1 || pressed_2) 
            {
                led_blue = 1;
                mvc();
            }
            break;
        case rest_biceps_leg:
            SubstateTransition();
            muscle = 4;
            led_green = 0;
            led_blue = 0;
            if (pressed_1 || pressed_2) 
            {
                led_green = 1;
                led_blue = 1;
                led_red = 0;
                rest();
            }
            break;
        case mvc_biceps_leg:
            SubstateTransition();
            muscle = 5;
            led_red = 0;
            led_green = 0;
            led_blue = 0;
            if (pressed_1 || pressed_2) 
            {
                led_green = 1;
                led_blue = 1;
                led_red = 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()
{
    pressed_1 = false;
    pressed_2 = false;
    if (StateChanged) {
        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");
    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.1);
    //TickerStateMachine.attach(StateMachine,1.00f);
    while(true) {
        StateMachine();
    }
}