File content as of revision 0:54c54952ee82:

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

AnalogIn EMG_bicepsright(A0);
AnalogIn EMG_bicepsleft(A1);
AnalogIn EMG_tricepsright(A2);
AnalogIn EMG_tricepsleft(A3);
HIDScope scope(4);
Ticker EMG_Control;

// Filter1 = High pass filter tot 20 Hz
double ah1_v1=0, ah1_v2=0, ah2_v1=0, ah2_v2=0;
double bh1_v1=0, bh1_v2=0, bh2_v1=0, bh2_v2=0;
double ch1_v1=0, ch1_v2=0, ch2_v1=0, ch2_v2=0; 
double dh1_v1=0, dh1_v2=0, dh2_v1=0, dh2_v2=0;
const double fh1_a1=-0.50701081158, fh1_a2=0.00000000000, fh1_b0= 1, fh1_b1=-1, fh1_b2=0;
const double fh2_a1=-1.24532140600, fh2_a2=0.54379713891, fh2_b0= 1, fh2_b1=-2, fh2_b2=1;
// Filter2 = Low pass filter na 60 Hz
double al1_v1=0, al1_v2=0, al2_v1=0, al2_v2=0;
double bl1_v1=0, bl1_v2=0, bl2_v1=0, bl2_v2=0;
double cl1_v1=0, cl1_v2=0, cl2_v1=0, cl2_v2=0;
double dl1_v1=0, dl1_v2=0, dl2_v1=0, dl2_v2=0;
const double fl1_a1=0.15970686439, fl1_a2=0.00000000000, fl1_b0= 1, fl1_b1=1, fl1_b2=0;
const double fl2_a1=0.42229458338, fl2_a2=0.35581444792, fl2_b0= 1, fl2_b1=2, fl2_b2=1;
// Filter3 = Notch filter at 50 Hz
double ano1_v1=0, ano1_v2=0, ano2_v1=0, ano2_v2=0, ano3_v1=0, ano3_v2=0;
double bno1_v1=0, bno1_v2=0, bno2_v1=0, bno2_v2=0, bno3_v1=0, bno3_v2=0;
double cno1_v1=0, cno1_v2=0, cno2_v1=0, cno2_v2=0, cno3_v1=0, cno3_v2=0;
double dno1_v1=0, dno1_v2=0, dno2_v1=0, dno2_v2=0, dno3_v1=0, dno3_v2=0;
const double fno1_a1 = -0.03255814954, fno1_a2= 0.92336486961, fno1_b0= 1, fno1_b1= -0.03385540628, fno1_b2= 1;
const double fno2_a1 = 0.03447359684, fno2_a2= 0.96095720701, fno2_b0= 1, fno2_b1= -0.03385540628, fno2_b2= 1;
const double fno3_a1 =  -0.10078591122, fno3_a2= 0.96100189080, fno3_b0= 1, fno3_b1= -0.03385540628, fno3_b2= 1;
// Filter4 = Lowpass filter at 5 Hz
double alp1_v1=0, alp1_v2=0, alp2_v1=0, alp2_v2=0;
double blp1_v1=0, blp1_v2=0, blp2_v1=0, blp2_v2=0;
double clp1_v1=0, clp1_v2=0, clp2_v1=0, clp2_v2=0;
double dlp1_v1=0, dlp1_v2=0, dlp2_v1=0, dlp2_v2=0;
const double flp1_a1=-0.86962685441, flp1_a2=0.00000000000, flp1_b0= 1, flp1_b1=1, flp1_b2=0;
const double flp2_a1=-1.85211666729, flp2_a2=0.87021679713, flp2_b0= 1, flp2_b1=2, flp2_b2=1;
double y1, y2, y3, y4, y5, y6, y7, y8, y9, y10, y11, y12, y13, y14, y15, y16, y17, y18, y19, y20, y21, y22, y23, y24, y25, y26, y27, y28, y29, y30, y31, y32, y33, y34, y35, y36; 
double u1, u2, u3, u4, u5, u6, u7,u8 , u9, u10, u11, u12, u13, u14, u15, u16, u17, u18, u19, u20, u21, u22, u23, u24, u25, u26, u27, u28, u29, u30, u31, u32, u33, u34, u35, u36;
double final_filter1, final_filter2, final_filter3, final_filter4;

// 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 MyController()
{
    // Biceps left
    u1 = EMG_bicepsright.read();
    //Highpass
    y1 = biquad (u1, ah1_v1, ah1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
    u2 = y1;
    y2 = biquad (u2, ah2_v1, ah2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
    //Lowpass
    u3 = y2;
    y3 = biquad (u3, al1_v1, al1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
    u4 = y3;
    y4 = biquad (u4, al2_v1, al2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
    // Notch
    u5 = y4;
    y5 = biquad (u5, ano1_v1, ano1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
    u6 = y5;
    y6 = biquad (u6, ano2_v1, ano2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
    u7 = y6;
    y7 = biquad (u7, ano3_v1, ano3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
    // Rectify sample
    y8 = fabs(y7);
    // Make it smooth
    u8 = y8;
    y9 = biquad (u8, alp1_v1, alp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1* 0.065187, flp1_b2* 0.065187);
    u9 = y9;
    final_filter1 = biquad(u9, alp2_v1, alp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);
    //Biceps right
    u10 = EMG_bicepsleft.read();
    //Highpass
    y10 = biquad (u10, bh1_v1, bh1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
    u11 = y10;
    y11 = biquad (u11, bh2_v1, bh2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
    //Lowpass
    u12 = y11;
    y12 = biquad (u12, bl1_v1, bl1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
    u13 = y12;
    y13 = biquad (u13, bl2_v1, bl2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
    // Notch
    u14 = y13;
    y14 = biquad (u14, bno1_v1, bno1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
    u15 = y14;
    y15 = biquad (u15, bno2_v1, bno2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
    u16 = y15;
    y16 = biquad (u16, bno3_v1, bno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
    // Rectify sample
    y17 = fabs(y16);
    // Make it smooth
    u17 = y17;
    y18 = biquad (u17, blp1_v1, blp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1*0.065187, flp1_b2*0.065187);
    u18 = y18;
    final_filter2 = biquad(u18, blp2_v1, blp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);
    /// EMG Filter left leg
    u19 = EMG_tricepsright.read();
    //Highpass
    y19 = biquad (u19, ch1_v1, ch1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
    u20 = y19;
    y20 = biquad (u20, ch2_v1, ch2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
    //Lowpass
    u21 = y20;
    y21 = biquad (u21, cl1_v1, cl1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
    u22 = y21;
    y22 = biquad (u22, cl2_v1, cl2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
    // Notch
    u23 = y22;
    y23 = biquad (u23, cno1_v1, cno1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
    u24 = y23;
    y24 = biquad (u24, cno2_v1, cno2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
    u25 = y24;
    y25 = biquad (u25, cno3_v1, cno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
    // Rectify sample
    y26 = fabs(y25);
    // Make it smooth
    u26 = y26;
    y27 = biquad (u26, clp1_v1, clp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1*0.065187, flp1_b2*0.065187);
    u27 = y27;
    final_filter3 = biquad(u27, clp2_v1, clp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);
    // EMG filter right leg
    u28 = EMG_tricepsleft.read();
    //Highpass
    y28 = biquad (u8, dh1_v1, dh1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
    u29 = y28;
    y29 = biquad (u29, dh2_v1, dh2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
    //Lowpass
    u30 = y29;
    y30 = biquad (u30, dl1_v1, dl1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
    u31 = y30;
    y31 = biquad (u31, dl2_v1, dl2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
    // Notch
    u32 = y31;
    y32 = biquad (u32, dno1_v1, dno1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
    u33 = y32;
    y33 = biquad (u33, dno2_v1, dno2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
    u34 = y33;
    y34 = biquad (u34, dno3_v1, dno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
    // Rectify sample
    y35 = fabs(y34);
    // Make it smooth
    u35 = y35;
    y36 = biquad (u35, dlp1_v1, dlp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1*0.065187, flp1_b2*0.065187);
    u36 = y36;
    final_filter4 = biquad(u36, dlp2_v1, dlp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);

    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(MyController,5e4);
}