File content as of revision 38:f45aa515f625:

#include "mbed.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "HIDScope.h"
#include <math.h>
#include "QEI.h"
//
//ATTENTION:    set mBed to version 151
//              set QEI to version 0
//              set MODSERIAL to version 44
//              set HIDScope to version 7
//              set biquadFilter to version 7

AnalogIn emg0_in            (A0);                                                                //First raw EMG signal input: calve muscle
AnalogIn emg1_in            (A1);                                                                //Second raw EMG signal input: biceps muscle 1
AnalogIn emg2_in            (A2);                                                                //Third raw EMG signal input: biceps muscle 2

InterruptIn button1         (D10);                  
InterruptIn button2         (D11);

DigitalOut directionpin1    (D4);
DigitalOut directionpin2    (D7);

PwmOut pwmpin1              (D5);
PwmOut pwmpin2              (D6);

DigitalOut ledr             (LED_RED);
DigitalOut ledb             (LED_BLUE);
DigitalOut ledg             (LED_GREEN);


MODSERIAL pc(USBTX, USBRX);                                                                     //Serial communication to see if the code works step by step, turn on if hidscope is off
QEI encoder2 (D9, D8, NC, 8400,QEI::X4_ENCODING);
QEI encoder1 (D12, D13, NC, 8400,QEI::X4_ENCODING);

//--------------Tickers------------------------------------------------------------------------------------------------------------------------------//

Ticker      func_tick; 
Ticker      movag_tick;
Ticker      emg_tick; 
Ticker      print_tick;
                       
                       
//--------------Global Variables---------------------------------------------------------------------------------------------------------------------//

//Ticker frequencies
const float T   = 0.002f;                                                                        //Ticker period EMG, engine control
const float T2  = 0.2f;                                                                          //Ticker print function

//EMG filter
double emg0_filt, emg1_filt, emg2_filt;                                                          //Variables for filtered EMG data channel 0, 1 and 2
double emg0_raw, emg1_raw, emg2_raw;
double emg0_filt_x, emg1_filt_x, emg2_filt_x;
const int windowsize = 150;                                                                      //Size of the array over which the moving average (MovAg) is calculated
double sum, sum1, sum2, sum3;                                                                    //Variables used to sum elements in array
double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize];                //Empty arrays to calculate MovAg
double movAg0, movAg1, movAg2;                                                                   //Outcome of MovAg

//Calibration variables
int x = -1;                                                                                      //Start switch, colour LED is blue.
int emg_cal = 0;                                                                                 //If emg_cal is set to 1, motors can begin to work in this code
const int sizeCal = 1500;                                                                        //Size of the dataset used for calibration
double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal];                               //Arrays to put the dataset of the calibration in
double Mean0,Mean1,Mean2;                                                                        //Average of maximum contraction: Threshold values
double Threshold0, Threshold1, Threshold2; 

//Biquad                                                                                         //Variables for the biquad band filters
BiQuadChain emg0filter;
BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );              //Notch filter biquad coefficients

BiQuadChain emg1filter;
BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );              

BiQuadChain emg2filter;
BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );              


//Variables PID controller
double PI = 3.14159;                                                                            //Pi value

double Kp1 = 20.0;                                                                              //Proportional gain motor 1
double Ki1 = 1.02;                                                                              //Integrative term motor 1
double Kd1 = 1.0;                                                                               //Differential term motor 1
double encoder_radians1=0;                                                                      //Inital encoder value motor 1
double err_integral1 = 0;                                                                       //Initial error integral value motor 1
double err_prev1, err_prev2;                                                                    //Variables called previous error motor 1 and motor 2
double err1, err2;                                                                              //Variables called current error motor 1 and motor 2
BiQuad LowPassFilterDer1( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 );   //Lowpass differential term: Sample frequency 500 Hz, cutoff 20Hz low pass

double Kp2 = 20.0;                                  /                                           //Motor 2
double Ki2 = 1.02;
double Kd2 = 1.0;
double encoder_radians2=0;
double err_integral2 = 0;
double u1, u2;
BiQuad LowPassFilterDer2( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 );

// Inverse Kinematica variables
//const double L1 = 0.208;                                                                      //Height of the base assembly
//const double L2 = 0.288;                                                                      //Height of joint 2
const double L3 = 0.212;                                                                        //Length of the rotating arm
const double r_trans = 0.035;                                                                   //Radius of translational gear

// Variërende variabelen inverse kinematics: 
double q1ref = 0.0;                                                                             //Current motor angle of joint 1, initial value = 0
double q2ref = 0.0;                                                                             //Current motor angle of joint 2, initial value = 0
double v_x;                                                                                     //Desired velocity of end effector in x direction --> Determined by EMG signals
double v_y;                                                                                     //Desired velocity of end effector in y direction --> Determined by EMG signals

double q1_dot=0.0;                                                                              //Required angular velocity of motor 1 to reach v_des
double q2_dot=0.0;                                                                              //Required angular velocity of motor 2 to reach v_des

double q1_ii=0.0;                                                                               //New reference angle for joint 1, becomes new q1ref  
double q2_ii=0.0;                                                                               //New reference angke for joint 2, becomes new q2ref

double q1_motor;                                                                                //Reference motor angle 1, input PID control
double q2_motor;                                                                                //Reference motor angle 2, input PID control

//--------------Functions----------------------------------------------------------------------------------------------------------------------------//


//------------------ Filter EMG + Calibration EMG --------------------------------------------------------------------------------------------------//

void switch_to_calibrate()
{
    //Every time function gets called, x increases. Every button press --> new calibration state.
    //Starts with x = -1. So when function gets called 1 time, x = 0.  In the end, x = 4 will reset to -1.
    
    x++;                                                                                       
                                

    if(x==0)                                                                                   //If x = 0, led is red
    {
        ledr = 0;
        ledb = 1;
        ledg = 1;
    }
    else if (x==1)                                                                            //If x = 1, led is blue
    {
        ledr = 1;
        ledb = 0;
        ledg = 1;
    }
    else if (x==2)                                                                            //If x = 2, led is green
    {
        ledr = 1;
        ledb = 1;
        ledg = 0;
    }
    else                                                                                      //If x = 3 or 4, led is white
    {
        ledr = 0;
        ledb = 0;
        ledg = 0;
    }
   
    if(x==4)                                                                                  //Reset back to x = -1
    {
        x = -1;
        emg_cal=0;                                                                            //Reset, motors off
    }
}
    
        
void calibrate(void)
{
    switch(x)
    {
        case 0:                                                                               //If calibration state 0:
        {
            sum = 0.0;
            for(int j = 0; j<=sizeCal-1; j++)                                                 //Array filled with datapoints from the EMGfilter signal of muscle 0
            {
                StoreCal0[j] = emg0_filt;
                sum+=StoreCal0[j];
                wait(0.001f);                                                               
            }
            Mean0       = sum/sizeCal;                                                        //Calculate mean of the datapoints in the calibration set
            Threshold0  = Mean0*0.5;                                                          //Threshold calculation calve = 0.5*mean                                         
            break;                                                                            //Stop. Threshold is calculated.
        }
        case 1:                                                                               //If calibration state 1:
        {
            sum = 0.0;                                  
            for(int j = 0; j<=sizeCal-1; j++)                                                 //Array filled with datapoints from the EMGfilter signal of muscle 1
            {
                StoreCal1[j] = emg1_filt;
                sum+=StoreCal1[j];
                wait(0.001f);
            }
            Mean1       = sum/sizeCal;
            Threshold1  = Mean1/2;                      
            break;
        } 
        case 2:                                                                               //If calibration state 2:
        {
            sum = 0.0;
            for(int j = 0; j<=sizeCal-1; j++)                                                 //Array filled with datapoints from the EMGfilter signal of muscle 2
            {
                StoreCal2[j] = emg2_filt;
                sum+=StoreCal2[j];
                wait(0.001f);
            }
            Mean2       = sum/sizeCal;
            Threshold2  = Mean2/2;                    
            break;
        }
        case 3:                                                                              //EMG is calibrated, robot can be set to Home position.
        {
            emg_cal = 1;                                                                     //This is the setting for which the motors can begin turning in this code
            
            wait(0.001f);
            break;
        }
        default:                                                                             //Ensures nothing happens if x is not 0,1 or 2
        {
            break;
        }
    }
}

void EMGFilter0()
{   
    emg0_raw      = emg0_in.read();                                                          //Give name to raw EMG0 data
    emg0_filt_x   = emg0filter.step(emg0_raw);                                               //Use biquad chain to filter raw EMG data
    emg0_filt     = abs(emg0_filt_x);                                                        //Rectifier
}

void EMGFilter1()
{
    emg1_raw      = emg1_in.read();                                                          //Give name to raw EMG1 data
    emg1_filt_x   = emg1filter.step(emg1_raw);                                               //Use biquad chain to filter raw EMG data
    emg1_filt     = abs(emg1_filt_x);                                                        //Rectifier
}

void EMGFilter2()
{
    emg2_raw      = emg2_in.read();                                                          //Give name to raw EMG1 data
    emg2_filt_x   = emg2filter.step(emg2_raw);                                               //Use biquad chain to filter raw EMG data
    emg2_filt     = abs(emg2_filt_x);                                                        //Rectifier
}
 
void MovAg()                                                                                 //Calculate moving average (MovAg)
{
    for (int i = windowsize-1; i>=0; i--)                                                    //Make arrays for the last datapoints of the filtered signals
    {
        StoreArray0[i] = StoreArray0[i-1];                                                   //Shifts the i'th element one place to the right, this makes it "rolling or moving" average.
        StoreArray1[i] = StoreArray1[i-1];
        StoreArray2[i] = StoreArray2[i-1];
    }
    
    StoreArray0[0] = emg0_filt;                                                              //Stores the latest datapoint of the filtered signal in the first element of the array
    StoreArray1[0] = emg1_filt;
    StoreArray2[0] = emg2_filt;
    
    sum1 = 0.0;
    sum2 = 0.0;
    sum3 = 0.0;
    
    for(int a = 0; a<= windowsize-1; a++)                                                   //Sums the elements in the arrays
    {
        sum1 += StoreArray0[a];
        sum2 += StoreArray1[a];
        sum3 += StoreArray2[a];
    }
    
    movAg0 = sum1/windowsize;                                                               //Calculates an average in the array
    movAg1 = sum2/windowsize;
    movAg2 = sum3/windowsize;
}

void emg_filtered()                                                                         //Call all filter functions
{
    EMGFilter0();
    EMGFilter1();
    EMGFilter2();
}

//---------PID controller 1 + 2 + motor control 1 & 2-----------------------------------------------------------------------------------------------//
void PID_control1()
{
    // Proportional part:
    double u_k1 = Kp1 * err1;                                                              //Proportional gain times calculated error

    //Integral part  
      err_integral1 = err_integral1 + err1 * T;                                            //Adds the error*T 
      double u_i1 = Ki1 * err_integral1;                                                   //Integral term times the integral
    
    // Derivative part
      double err_derivative1 = (err1 - err_prev1)/T;                                       //Error - previous error /T
      double filtered_err_derivative1 = LowPassFilterDer1.step(err_derivative1);           //Filter the derivative term for stabilization
      double u_d1 = Kd1 * filtered_err_derivative1;                                        //Derivative term times the derivative error
      err_prev1 = err1;                                                                    //Sets the current error to previous error (remember)
      
    // Sum all parts and return it
      u1 = u_k1 + u_i1 + u_d1;      
}

void PID_control2()
{
    // Proportional part:
    double u_k2 = Kp2 * err2;

    //Integral part  
      err_integral2 = err_integral2 + err2 * T;
      double u_i2 = Ki2 * err_integral2;
    
    // Derivative part
      double err_derivative2 = (err2 - err_prev2)/T;
      double filtered_err_derivative2 = LowPassFilterDer2.step(err_derivative2);
      double u_d2 = Kd2 * filtered_err_derivative2;
      err_prev2 = err2;
      
    // Sum all parts and return it
      u2 = u_k2 + u_i2 + u_d2;  
}
void engine_control1()                                                                     //Engine 1 is translational joint, connected with left side pins
{
        encoder_radians1 = (double)encoder1.getPulses()*(2.0*PI)/8400.0;
        err1 = q1_motor - encoder_radians1;                                                //Calculate error between reference angle 1 and current angle 1
        PID_control1();                                                                    //PID 1 controller function call
        pwmpin1 = fabs(u1);                                                                //Set speed motor 1 
        directionpin1.write(u1<0);                                                         //Set direction motor 1
}

void engine_control2()                                                                     //Engine 2 is rotational joint, connected with right side wires
{
        encoder_radians2 = (float)encoder2.getPulses()*(2.0*PI)/8400.0;
        err2 = q2_motor - encoder_radians2;                                                //Calculate error between reference angle 2 and current angle 2
        PID_control2();                                                                    //PID 2 controller function call
        pwmpin2 = fabs(u2);                                                                //Set speed motor 2 
        directionpin2.write(u2>0);                                                         //Set direction motor 2
}


//------------------ Inversed Kinematics -----------------------------------------------------------------------------------------------------------//

void inverse_kinematics()
{                              

    q1_dot = (v_x*cos(q2ref) + v_y*sin(q2ref))/cos(q2ref);                                 //Calculate desired angular velocity of motor 1              
    q2_dot = v_y/(L3*cos(q2ref));                                                          //Calculate desired angular velocity of motor 2
            
    q1_ii = q1ref + q1_dot*T;                                                              //Calculate new reference angle of joint 1, from current angle and desired angular velocity times ticker time
    q2_ii = q2ref + q2_dot*T;                                                              //Calculate new reference angle of joint 2, from current angle and desired angular velocity times ticker time
        
    q1ref = q1_ii;                                                                         //Replace qref by newly calculated reference angle
    q2ref = q2_ii;                                                                         
    
    q1_motor = -q1ref/r_trans;                                                             //Calculate reference motor angle 1, corrected for translational joint --> input PID control
    q2_motor = q2ref*5.0;                                                                  //Calculate reference motor angle 2, corrected for gear ratio 1:5 ---> input PID control
    
    engine_control1();                                                                     //Call engine_control 1 function
    engine_control2();                                                                     //Call engine_control 2 function

}

void v_des_calculate_qref()
{
    while(emg_cal==1)                                                                      //After calibration is finished, emg_cal will be 1. Otherwise 0. 
    { 
                if(movAg1>Threshold1 && movAg0<Threshold0)                                 //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is off (movAg0)
                {
                    v_y = 0.0;
                    v_x = 0.05;                                                            //Movement in +x direction
                       
                    ledr = 0;                                                              //Led is red
                    ledb = 1;
                    ledg = 1;
                }
                else if(movAg2>Threshold2 && movAg0<Threshold0)                            //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is off (movAg0)
                {
                    v_y = 0.05;                                                            //Movement in +y direction
                    v_x = 0.0;
                    
                    ledr = 1;                                                              //Led is green
                    ledb = 1;
                    ledg = 0;
                }
               
                else if(movAg0>Threshold0 && movAg1>Threshold1)                            //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is on (movAg0)
                {
                    v_y = 0.0;                                                             //Movement in -x direction
                    v_x = -0.05;
                    
                    ledr = 0;                                                              //Led is purple
                    ledb = 0;
                    ledg = 1;
                }
                
                else if(movAg0>Threshold0 && movAg2>Threshold2)                            //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is on (movAg0)
                {
                    v_y = -0.05;                                                           //Movement in -y direction
                    v_x = 0.0;
                    
                    ledr = 1;                                                              //Led is blue
                    ledb = 0;
                    ledg = 1;
                }
                else                                                                       //If not higher than any threshold, motors will not turn at all
                {                    
                    v_x = 0;
                    v_y = 0;
                    
                    ledr = 0;                                                              //Led is white
                    ledb = 0;
                    ledg = 0;
                }
                
        inverse_kinematics();                                                             //Call inverse kinematics function
        
        break;
        }
}

//------------------ Start main function -----------------------------------------------------------------------------------------------------------//

int main()
{         
        pwmpin1.period_us(60);                                                           //60 microseconds PWM period, 16.7 kHz 

        emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( &notch1 ); //Attach biquad elements to chain
        emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( &notch2 );
        emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( &notch3 );
        
        emg_tick.attach(&emg_filtered,T);                                                //EMG signals filtered + moving average every T sec.
        movag_tick.attach(&MovAg,T);
        func_tick.attach(&v_des_calculate_qref,T);                                       //v_des determined every T
        
        button1.rise(switch_to_calibrate);                                               //Switch state of calibration (which muscle)
        button2.rise(calibrate);                                                         //Calibrate threshold for 3 muscles
        
    while(true)
    {
        ;    
    }      
}