File content as of revision 8:63a7b61ca2ae:

#include "mbed.h"
#include "HIDScope.h"

AnalogIn EMG_bicepsleft(A0);
AnalogIn EMG_bicepsright (A1);
AnalogIn EMG_legleft (A2);
AnalogIn EMG_legright (A3);
HIDScope scope(4);
Timer Timer_Calibration;
Ticker EMG_Control;

bool Cali;
double TimeCali = 5;

// Filter1 = High pass filter tot 20 Hz
double fh1_v1=0, fh1_v2=0, fh2_v1=0, fh2_v2=0;
const double fh1_a1=-0.84909054461, fh1_a2=0.00000000000, fh1_b0= 1, fh1_b1=-1, fh1_b2=0;
const double fh2_a1=-1.82553264091, fh2_a2=0.85001416809, fh2_b0= 1, fh2_b1=-2, fh2_b2=1;
// Filter2 = Low pass filter na 60 Hz
double fl1_v1=0, fl1_v2=0, fl2_v1=0, fl2_v2=0;
const double fl1_a1=-0.66979455390, fl1_a2=0.00000000000, fl1_b0= 1, fl1_b1=1, fl1_b2=0;
const double fl2_a1=-1.55376616139, fl2_a2=0.68023470431, fl2_b0= 1, fl2_b1=2, fl2_b2=1;
// Filter3 = Notch filter at 50 Hz
double fno1_v1=0, fno1_v2=0, fno2_v1=0, fno2_v2=0, fno3_v1=0, fno3_v2=0;
const double fno1_a1 = -1.87934916386, fno1_a2= 0.97731851355, fno1_b0= 1, fno1_b1= -1.90090686046, fno1_b2= 1;
const double fno2_a1 = -1.88341028603, fno2_a2= 0.98825147717, fno2_b0= 1, fno2_b1= -1.90090686046, fno2_b2= 1;
const double fno3_a1 = -1.89635403726, fno3_a2= 0.98894004849, fno3_b0= 1, fno3_b1= -1.90090686046, fno3_b2= 1;
// Filter4 = Lowpass filter at 5 Hz
double flp1_v1=0, flp1_v2=0, flp2_v1=0, flp2_v2=0;
const double flp1_a1=-0.97922725527, flp1_a2=0.00000000000, flp1_b0= 1, flp1_b1=1, flp1_b2=0;
const double flp2_a1=-1.97879353121, flp2_a2=0.97922951943, flp2_b0= 1, flp2_b1=2, flp2_b2=1;
double y1, y2, y3, y4, y5, y6, y7, y8, y9, u1, u2, u3, u4, u5, u6, u7,u8 , u9 ;
double final_filter1, final_filter2, final_filter3, final_filter4;
double fblmax = 0;
double fbrmax = 0;
double fllmax = 0;
double flrmax = 0;



// Standaard formule voor het biquad filter
double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)

{
    double v = u - a1*v1 - a2*v2;
    double y = b0*v + b1*v1 + b2*v2;
    v2=v1;
    v1=v;
    return y;
}

void Filters_bicepsleft()
{
    // Biceps left
    u1 = EMG_bicepsleft.read();
    //Highpass
    y1 = biquad (u1, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547);
    u2 = y1;
    y2 = biquad (u2, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885);
    //Lowpass
    u3 = y2;
    y3 = biquad (u3, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103);
    u4 = y3;
    y4 = biquad (u4, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617);
    // Notch
    u5 = y4;
    y5 = biquad (u5, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206);
    u6 = y5;
    y6 = biquad (u6, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2);
    u7 = y6;
    y7 = biquad (u7, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227);
    // Rectify sample
    y8 = fabs(y7);
    // Make it smooth
    u8 = y8;
    y9 = biquad (u8, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386);
    u9 = y9;
    final_filter1 = biquad(u9, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109);
    //Biceps right
    u10 = EMG_bicepsright.read();
    //Highpass
    y10 = biquad (u10, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547);
    u11 = y10;
    y11 = biquad (u11, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885);
    //Lowpass
    u12 = y11;
    y12 = biquad (u12, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103);
    u13 = y12;
    y13 = biquad (u13, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617);
    // Notch
    u14 = y13;
    y14 = biquad (u14, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206);
    u15 = y14;
    y15 = biquad (u15, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2);
    u16 = y15;
    y16 = biquad (u16, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227);
    // Rectify sample
    y17 = fabs(y16);
    // Make it smooth
    u17 = y17;
    y18 = biquad (u17, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386);
    u18 = y18;
    final_filter2 = biquad(u18, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109);
    /// EMG Filter left leg
    u19 = EMG_legleft.read();
    //Highpass
    y19 = biquad (u19, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547);
    u20 = y19;
    y20 = biquad (u20, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885);
    //Lowpass
    u21 = y20;
    y21 = biquad (u21, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103);
    u22 = y21;
    y22 = biquad (u22, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617);
    // Notch
    u23 = y22;
    y23 = biquad (u23, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206);
    u24 = y23;
    y24 = biquad (u24, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2);
    u25 = y24;
    y25 = biquad (u25, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227);
    // Rectify sample
    y26 = fabs(y7);
    // Make it smooth
    u26 = y26;
    y27 = biquad (u26, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386);
    u27 = y27;
    final_filter3 = biquad(u27, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109);
    // EMG filter right leg
    u28 = EMG_legright.read();
    //Highpass
    y28 = biquad (u8, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547);
    u29 = y28;
    y29 = biquad (u29, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885);
    //Lowpass
    u30 = y29;
    y30 = biquad (u30, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103);
    u31 = y30;
    y31 = biquad (u31, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617);
    // Notch
    u32 = y31;
    y32 = biquad (u32, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206);
    u33 = y32;
    y33 = biquad (u33, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2);
    u34 = y33;
    y34 = biquad (u34, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227);
    // Rectify sample
    y35 = fabs(y34);
    // Make it smooth
    u35 = y35;
    y36 = biquad (u35, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386);
    u36 = y36;
    final_filter4 = biquad(u36, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109);

    if (Cali == true) {
        if (final_filter1 >= fblmax) {
            fblmax=final_filter1;
        }
        if (final_filter2 >= fbrmax) {
            fbrmax = final_filter2;
        }
        if (final_filter3 >= fllmax) {
            fllmax=final_filter3;
        }
        if (final_filter4 >= flrmax) {
            flrmax=final_filter4;
        }
        Tbl = 0.8*fblmax;
        Tbr = 0.8*fbrmax;
        Tll = 0.8*fllmax;
        Tlr = 0.8*flrmax;
        wait (3)
    }
}

// Send signals to HIDScope
void Filters_EMG ()
{
    scope.set (0,final_filter1);
    scope.set (1,final_filter2);
    scope.set (2,final_filter3);
    scope.set (3,final_filter4);
    scope.send ();
}

int main ()
{
    EMG_Control.attach_us(Filters_EMG,1e3);
    Timer_Calibration.start();
    if (Timer_Calibration < TimerCali) {
        Cali = true;
    }
    if (Timer_Calibration > TimerCali) {
        Cali = false;
    }
    Timer_Calibration.stop();
    Timer_Calibration.reset();
}




}