File content as of revision 24:6661dd6a1718:

#include "mbed.h"
#include "HIDScope.h"
#include "MODSERIAL.h"
#include "biquadFilter.h"
#include "MAF.h"

//Define objects
    AnalogIn    EMG_left(A0); //Analog input
    AnalogIn    EMG_right(A1);   
    Ticker      SampleEMG;
    Ticker      ScopeTimer;
    Ticker      serial;
    Ticker      MovingAverage;
    HIDScope    scope(6);
    DigitalOut led(LED_RED);
    MODSERIAL pc(USBTX, USBRX);
    

// Declaring variables
    double EMG_L_f_v1 = 0, EMG_L_f_v2 = 0;
    double EMG_L_fh=0;
    double EMG_left_value;
    double EMG_left_f1;
    double EMG_left_f2;
    double EMG_left_f3;
    double EMG_left_f4;
    double EMG_left_f5;
    double EMG_left_f6;
    double EMG_left_abs;
    double EMG_right_value;
    double EMG_right_f1;
    double EMG_right_f2;
    double EMG_right_abs;
    double Threshold1 = 0.03;
    double Threshold2 = 0.06;
    
    int N = 20;
    double MAF_left[20];
    double EMG_left_MAF;
    
// coëfficiënten
    const double BiGainEMG_H1 = 0.795375, BiGainEMG_H2 = 0.895763;
    const double EMGH1_a1 = -1.56308931068*BiGainEMG_H1, EMGH1_a2 = 0.61765749583*BiGainEMG_H1, EMGH1_b0 = 1.0*BiGainEMG_H1, EMGH1_b1 = -1.99909075151*BiGainEMG_H1, EMGH1_b2 = 1.0*BiGainEMG_H1; //coefficients for high-pass filter
    const double EMGH2_a1 = -1.75651417587*BiGainEMG_H2, EMGH2_a2 = 0.82183182692*BiGainEMG_H2, EMGH2_b0 = 1.0*BiGainEMG_H2, EMGH2_b1 = -1.99470632157*BiGainEMG_H2, EMGH2_b2 = 1.0*BiGainEMG_H2; //coefficients for high-pass filter
    
    const double BiGainEMG_L1=0.959332, BiGainEMG_L2 = 0.223396;
    const double EMGL1_a1 = -1.55576653052*BiGainEMG_L1, EMGL1_a2 = 0.61374320375*BiGainEMG_L1, EMGL1_b0 = 1.0*BiGainEMG_L1, EMGL1_b1 = -0.90928276835*BiGainEMG_L1, EMGL1_b2 = 1.0*BiGainEMG_L1; // coefficients for low-pass filter
    const double EMGL2_a1 = -1.79696141922*BiGainEMG_L2, EMGL2_a2 = 0.85096669383*BiGainEMG_L2, EMGL2_b0 = 1.0*BiGainEMG_L2, EMGL2_b1 = -1.75825311060*BiGainEMG_L2, EMGL2_b2 = 1.0*BiGainEMG_L2; // coefficients for low-pass filter

    const double BiGainEMG_N1 = 1.0, BiGainEMG_N2 = 0.965081;
    const double EMGN1_a1 = -1.56858163035*BiGainEMG_N1, EMGN1_a2 = 0.96424138362*BiGainEMG_N1, EMGN1_b0 = 1.0*BiGainEMG_N1, EMGN1_b1 = -1.61854514265*BiGainEMG_N1, EMGN1_b2 = 1.0*BiGainEMG_N1; //coefficients for high-pass filter
    const double EMGN2_a1 = -1.61100357722*BiGainEMG_N2, EMGN2_a2 = 0.96592170538*BiGainEMG_N2, EMGN2_b0 = 1.0*BiGainEMG_N2, EMGN2_b1 = -1.61854514265*BiGainEMG_N2, EMGN2_b2 = 1.0*BiGainEMG_N2; //coefficients for high-pass filter
   
// Filter creation    
    biquadFilter EMG_highpass1 (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2);        // creates the high pass filter
    biquadFilter EMG_highpass2 (EMGH2_a1, EMGH2_a2, EMGH2_b0, EMGH2_b1, EMGH2_b2);
    biquadFilter EMG_lowpass1 (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2);         // creates the low pass filter
    biquadFilter EMG_lowpass2 (EMGL2_a1, EMGL2_a2, EMGL2_b0, EMGL2_b1, EMGL2_b2);  
    biquadFilter EMG_notch1 (EMGN1_a1, EMGN1_a2, EMGN1_b0, EMGN1_b1, EMGN1_b2);           // creates the notch filter
    biquadFilter EMG_notch2 (EMGN2_a1, EMGN2_a2, EMGN2_b0, EMGN2_b1, EMGN2_b2);
        
// EMG filtering function
void EMGfilter() // Both EMG signals are filtered in one function and with the same filters
{
    EMG_left_value = EMG_left.read();
    EMG_left_f1 = EMG_highpass1.step(EMG_left_value);
    EMG_left_f2 = EMG_highpass2.step(EMG_left_f1);
    EMG_left_abs = fabs(EMG_left_f2);
    EMG_left_f3 = EMG_lowpass1.step(EMG_left_abs);
    EMG_left_f4 = EMG_lowpass2.step(EMG_left_f3);
    EMG_left_f5 = EMG_notch1.step(EMG_left_f4);
    EMG_left_f6 = EMG_notch1.step(EMG_left_f5);
        
//    EMG_right_value = EMG_right.read();
//    EMG_right_f1 = EMG_highpass.step(EMG_right_value);
//    EMG_right_f1 = EMG_lowpass.step(EMG_right_f1);
//    EMG_right_abs = fabs(EMG_right_f2);
}

// Movingaverage Filter
    void MovingAverageFilter()
    {
        int ArraySum;
        for (int a = 0; a<N; a++)
        {
            ArraySum = ArraySum + MAF_left[a];
        }
        EMG_left_MAF = ArraySum/N;
       // EMG_left_MAF = (MAF_left[0]+MAF_left[1]+MAF_left[2]+MAF_left[3]+MAF_left[5]+MAF_left[6]+MAF_left[7]+MAF_left[8]+MAF_left[9]+MAF_left[10]+MAF_left[11]+MAF_left[12]+MAF_left[13]+MAF_left[14]+MAF_left[15]+MAF_left[16]+MAF_left[17]+MAF_left[18]+MAF_left[19])/20;
        MAF_left[19] = MAF_left[18];
        MAF_left[18] = MAF_left[17];
        MAF_left[17] = MAF_left[16];
        MAF_left[16] = MAF_left[15];
        MAF_left[15] = MAF_left[14];
        MAF_left[14] = MAF_left[13];
        MAF_left[13] = MAF_left[12];
        MAF_left[12] = MAF_left[11];
        MAF_left[11] = MAF_left[10];
        MAF_left[10] = MAF_left[9];
        MAF_left[9] = MAF_left[8];
        MAF_left[8] = MAF_left[7];
        MAF_left[7] = MAF_left[6];
        MAF_left[6] = MAF_left[5];
        MAF_left[5] = MAF_left[4];
        MAF_left[4] = MAF_left[3];
        MAF_left[3] = MAF_left[2];
        MAF_left[2] = MAF_left[1];
        MAF_left[1] = MAF_left[0];
        MAF_left[0] = EMG_left_f6;
    }
    
// HIDScope
    void ScopeSend()
    {
        scope.set(0, EMG_left_value);
        scope.set(1, EMG_left_f2);
        scope.set(2, EMG_left_abs);
        scope.set(3, EMG_left_f4);
        scope.set(4, EMG_left_f6);
        scope.set(5, EMG_left_MAF);
        scope.send();
    }
    
int main()
{
    SampleEMG.attach(&EMGfilter, 0.002);
    ScopeTimer.attach(&ScopeSend, 0.002);
    MovingAverage.attach(&MovingAverageFilter, 0.002);
    while(1) 
    {
        if (EMG_left_f6 > Threshold1)
        {
            led.write(0);
        }
        
        else
        {
            led.write(1);
        }
    }
}