Richell Booyink / Mbed 2 deprecated EMG_TEST

4 EMG filters die werken en op HIDScope afgebeeld kunnen worden

Dependencies:   HIDScope mbed

main.cpp@0:54c54952ee82, 2015-10-26 (annotated)

Committer:
RichellBooyink
Date:
Mon Oct 26 13:42:20 2015 +0000
Revision:
0:54c54952ee82
4 filters worden afgebeeld op HIDScope en laten het gewenste filter zien;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RichellBooyink 0:54c54952ee82 1 #include "mbed.h"
RichellBooyink 0:54c54952ee82 2 #include "HIDScope.h"
RichellBooyink 0:54c54952ee82 3
RichellBooyink 0:54c54952ee82 4 AnalogIn EMG_bicepsright(A0);
RichellBooyink 0:54c54952ee82 5 AnalogIn EMG_bicepsleft(A1);
RichellBooyink 0:54c54952ee82 6 AnalogIn EMG_tricepsright(A2);
RichellBooyink 0:54c54952ee82 7 AnalogIn EMG_tricepsleft(A3);
RichellBooyink 0:54c54952ee82 8 HIDScope scope(4);
RichellBooyink 0:54c54952ee82 9 Ticker EMG_Control;
RichellBooyink 0:54c54952ee82 10
RichellBooyink 0:54c54952ee82 11 // Filter1 = High pass filter tot 20 Hz
RichellBooyink 0:54c54952ee82 12 double ah1_v1=0, ah1_v2=0, ah2_v1=0, ah2_v2=0;
RichellBooyink 0:54c54952ee82 13 double bh1_v1=0, bh1_v2=0, bh2_v1=0, bh2_v2=0;
RichellBooyink 0:54c54952ee82 14 double ch1_v1=0, ch1_v2=0, ch2_v1=0, ch2_v2=0;
RichellBooyink 0:54c54952ee82 15 double dh1_v1=0, dh1_v2=0, dh2_v1=0, dh2_v2=0;
RichellBooyink 0:54c54952ee82 16 const double fh1_a1=-0.50701081158, fh1_a2=0.00000000000, fh1_b0= 1, fh1_b1=-1, fh1_b2=0;
RichellBooyink 0:54c54952ee82 17 const double fh2_a1=-1.24532140600, fh2_a2=0.54379713891, fh2_b0= 1, fh2_b1=-2, fh2_b2=1;
RichellBooyink 0:54c54952ee82 18 // Filter2 = Low pass filter na 60 Hz
RichellBooyink 0:54c54952ee82 19 double al1_v1=0, al1_v2=0, al2_v1=0, al2_v2=0;
RichellBooyink 0:54c54952ee82 20 double bl1_v1=0, bl1_v2=0, bl2_v1=0, bl2_v2=0;
RichellBooyink 0:54c54952ee82 21 double cl1_v1=0, cl1_v2=0, cl2_v1=0, cl2_v2=0;
RichellBooyink 0:54c54952ee82 22 double dl1_v1=0, dl1_v2=0, dl2_v1=0, dl2_v2=0;
RichellBooyink 0:54c54952ee82 23 const double fl1_a1=0.15970686439, fl1_a2=0.00000000000, fl1_b0= 1, fl1_b1=1, fl1_b2=0;
RichellBooyink 0:54c54952ee82 24 const double fl2_a1=0.42229458338, fl2_a2=0.35581444792, fl2_b0= 1, fl2_b1=2, fl2_b2=1;
RichellBooyink 0:54c54952ee82 25 // Filter3 = Notch filter at 50 Hz
RichellBooyink 0:54c54952ee82 26 double ano1_v1=0, ano1_v2=0, ano2_v1=0, ano2_v2=0, ano3_v1=0, ano3_v2=0;
RichellBooyink 0:54c54952ee82 27 double bno1_v1=0, bno1_v2=0, bno2_v1=0, bno2_v2=0, bno3_v1=0, bno3_v2=0;
RichellBooyink 0:54c54952ee82 28 double cno1_v1=0, cno1_v2=0, cno2_v1=0, cno2_v2=0, cno3_v1=0, cno3_v2=0;
RichellBooyink 0:54c54952ee82 29 double dno1_v1=0, dno1_v2=0, dno2_v1=0, dno2_v2=0, dno3_v1=0, dno3_v2=0;
RichellBooyink 0:54c54952ee82 30 const double fno1_a1 = -0.03255814954, fno1_a2= 0.92336486961, fno1_b0= 1, fno1_b1= -0.03385540628, fno1_b2= 1;
RichellBooyink 0:54c54952ee82 31 const double fno2_a1 = 0.03447359684, fno2_a2= 0.96095720701, fno2_b0= 1, fno2_b1= -0.03385540628, fno2_b2= 1;
RichellBooyink 0:54c54952ee82 32 const double fno3_a1 = -0.10078591122, fno3_a2= 0.96100189080, fno3_b0= 1, fno3_b1= -0.03385540628, fno3_b2= 1;
RichellBooyink 0:54c54952ee82 33 // Filter4 = Lowpass filter at 5 Hz
RichellBooyink 0:54c54952ee82 34 double alp1_v1=0, alp1_v2=0, alp2_v1=0, alp2_v2=0;
RichellBooyink 0:54c54952ee82 35 double blp1_v1=0, blp1_v2=0, blp2_v1=0, blp2_v2=0;
RichellBooyink 0:54c54952ee82 36 double clp1_v1=0, clp1_v2=0, clp2_v1=0, clp2_v2=0;
RichellBooyink 0:54c54952ee82 37 double dlp1_v1=0, dlp1_v2=0, dlp2_v1=0, dlp2_v2=0;
RichellBooyink 0:54c54952ee82 38 const double flp1_a1=-0.86962685441, flp1_a2=0.00000000000, flp1_b0= 1, flp1_b1=1, flp1_b2=0;
RichellBooyink 0:54c54952ee82 39 const double flp2_a1=-1.85211666729, flp2_a2=0.87021679713, flp2_b0= 1, flp2_b1=2, flp2_b2=1;
RichellBooyink 0:54c54952ee82 40 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;
RichellBooyink 0:54c54952ee82 41 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;
RichellBooyink 0:54c54952ee82 42 double final_filter1, final_filter2, final_filter3, final_filter4;
RichellBooyink 0:54c54952ee82 43
RichellBooyink 0:54c54952ee82 44 // Standaard formule voor het biquad filter
RichellBooyink 0:54c54952ee82 45 double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
RichellBooyink 0:54c54952ee82 46
RichellBooyink 0:54c54952ee82 47 {
RichellBooyink 0:54c54952ee82 48 double v = u - a1*v1 - a2*v2;
RichellBooyink 0:54c54952ee82 49 double y = b0*v + b1*v1 + b2*v2;
RichellBooyink 0:54c54952ee82 50 v2=v1;
RichellBooyink 0:54c54952ee82 51 v1=v;
RichellBooyink 0:54c54952ee82 52 return y;
RichellBooyink 0:54c54952ee82 53 }
RichellBooyink 0:54c54952ee82 54
RichellBooyink 0:54c54952ee82 55 void MyController()
RichellBooyink 0:54c54952ee82 56 {
RichellBooyink 0:54c54952ee82 57 // Biceps left
RichellBooyink 0:54c54952ee82 58 u1 = EMG_bicepsright.read();
RichellBooyink 0:54c54952ee82 59 //Highpass
RichellBooyink 0:54c54952ee82 60 y1 = biquad (u1, ah1_v1, ah1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
RichellBooyink 0:54c54952ee82 61 u2 = y1;
RichellBooyink 0:54c54952ee82 62 y2 = biquad (u2, ah2_v1, ah2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
RichellBooyink 0:54c54952ee82 63 //Lowpass
RichellBooyink 0:54c54952ee82 64 u3 = y2;
RichellBooyink 0:54c54952ee82 65 y3 = biquad (u3, al1_v1, al1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
RichellBooyink 0:54c54952ee82 66 u4 = y3;
RichellBooyink 0:54c54952ee82 67 y4 = biquad (u4, al2_v1, al2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
RichellBooyink 0:54c54952ee82 68 // Notch
RichellBooyink 0:54c54952ee82 69 u5 = y4;
RichellBooyink 0:54c54952ee82 70 y5 = biquad (u5, ano1_v1, ano1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
RichellBooyink 0:54c54952ee82 71 u6 = y5;
RichellBooyink 0:54c54952ee82 72 y6 = biquad (u6, ano2_v1, ano2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
RichellBooyink 0:54c54952ee82 73 u7 = y6;
RichellBooyink 0:54c54952ee82 74 y7 = biquad (u7, ano3_v1, ano3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
RichellBooyink 0:54c54952ee82 75 // Rectify sample
RichellBooyink 0:54c54952ee82 76 y8 = fabs(y7);
RichellBooyink 0:54c54952ee82 77 // Make it smooth
RichellBooyink 0:54c54952ee82 78 u8 = y8;
RichellBooyink 0:54c54952ee82 79 y9 = biquad (u8, alp1_v1, alp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1* 0.065187, flp1_b2* 0.065187);
RichellBooyink 0:54c54952ee82 80 u9 = y9;
RichellBooyink 0:54c54952ee82 81 final_filter1 = biquad(u9, alp2_v1, alp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);
RichellBooyink 0:54c54952ee82 82 //Biceps right
RichellBooyink 0:54c54952ee82 83 u10 = EMG_bicepsleft.read();
RichellBooyink 0:54c54952ee82 84 //Highpass
RichellBooyink 0:54c54952ee82 85 y10 = biquad (u10, bh1_v1, bh1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
RichellBooyink 0:54c54952ee82 86 u11 = y10;
RichellBooyink 0:54c54952ee82 87 y11 = biquad (u11, bh2_v1, bh2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
RichellBooyink 0:54c54952ee82 88 //Lowpass
RichellBooyink 0:54c54952ee82 89 u12 = y11;
RichellBooyink 0:54c54952ee82 90 y12 = biquad (u12, bl1_v1, bl1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
RichellBooyink 0:54c54952ee82 91 u13 = y12;
RichellBooyink 0:54c54952ee82 92 y13 = biquad (u13, bl2_v1, bl2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
RichellBooyink 0:54c54952ee82 93 // Notch
RichellBooyink 0:54c54952ee82 94 u14 = y13;
RichellBooyink 0:54c54952ee82 95 y14 = biquad (u14, bno1_v1, bno1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
RichellBooyink 0:54c54952ee82 96 u15 = y14;
RichellBooyink 0:54c54952ee82 97 y15 = biquad (u15, bno2_v1, bno2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
RichellBooyink 0:54c54952ee82 98 u16 = y15;
RichellBooyink 0:54c54952ee82 99 y16 = biquad (u16, bno3_v1, bno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
RichellBooyink 0:54c54952ee82 100 // Rectify sample
RichellBooyink 0:54c54952ee82 101 y17 = fabs(y16);
RichellBooyink 0:54c54952ee82 102 // Make it smooth
RichellBooyink 0:54c54952ee82 103 u17 = y17;
RichellBooyink 0:54c54952ee82 104 y18 = biquad (u17, blp1_v1, blp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1*0.065187, flp1_b2*0.065187);
RichellBooyink 0:54c54952ee82 105 u18 = y18;
RichellBooyink 0:54c54952ee82 106 final_filter2 = biquad(u18, blp2_v1, blp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);
RichellBooyink 0:54c54952ee82 107 /// EMG Filter left leg
RichellBooyink 0:54c54952ee82 108 u19 = EMG_tricepsright.read();
RichellBooyink 0:54c54952ee82 109 //Highpass
RichellBooyink 0:54c54952ee82 110 y19 = biquad (u19, ch1_v1, ch1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
RichellBooyink 0:54c54952ee82 111 u20 = y19;
RichellBooyink 0:54c54952ee82 112 y20 = biquad (u20, ch2_v1, ch2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
RichellBooyink 0:54c54952ee82 113 //Lowpass
RichellBooyink 0:54c54952ee82 114 u21 = y20;
RichellBooyink 0:54c54952ee82 115 y21 = biquad (u21, cl1_v1, cl1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
RichellBooyink 0:54c54952ee82 116 u22 = y21;
RichellBooyink 0:54c54952ee82 117 y22 = biquad (u22, cl2_v1, cl2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
RichellBooyink 0:54c54952ee82 118 // Notch
RichellBooyink 0:54c54952ee82 119 u23 = y22;
RichellBooyink 0:54c54952ee82 120 y23 = biquad (u23, cno1_v1, cno1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
RichellBooyink 0:54c54952ee82 121 u24 = y23;
RichellBooyink 0:54c54952ee82 122 y24 = biquad (u24, cno2_v1, cno2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
RichellBooyink 0:54c54952ee82 123 u25 = y24;
RichellBooyink 0:54c54952ee82 124 y25 = biquad (u25, cno3_v1, cno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
RichellBooyink 0:54c54952ee82 125 // Rectify sample
RichellBooyink 0:54c54952ee82 126 y26 = fabs(y25);
RichellBooyink 0:54c54952ee82 127 // Make it smooth
RichellBooyink 0:54c54952ee82 128 u26 = y26;
RichellBooyink 0:54c54952ee82 129 y27 = biquad (u26, clp1_v1, clp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1*0.065187, flp1_b2*0.065187);
RichellBooyink 0:54c54952ee82 130 u27 = y27;
RichellBooyink 0:54c54952ee82 131 final_filter3 = biquad(u27, clp2_v1, clp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);
RichellBooyink 0:54c54952ee82 132 // EMG filter right leg
RichellBooyink 0:54c54952ee82 133 u28 = EMG_tricepsleft.read();
RichellBooyink 0:54c54952ee82 134 //Highpass
RichellBooyink 0:54c54952ee82 135 y28 = biquad (u8, dh1_v1, dh1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
RichellBooyink 0:54c54952ee82 136 u29 = y28;
RichellBooyink 0:54c54952ee82 137 y29 = biquad (u29, dh2_v1, dh2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
RichellBooyink 0:54c54952ee82 138 //Lowpass
RichellBooyink 0:54c54952ee82 139 u30 = y29;
RichellBooyink 0:54c54952ee82 140 y30 = biquad (u30, dl1_v1, dl1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
RichellBooyink 0:54c54952ee82 141 u31 = y30;
RichellBooyink 0:54c54952ee82 142 y31 = biquad (u31, dl2_v1, dl2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
RichellBooyink 0:54c54952ee82 143 // Notch
RichellBooyink 0:54c54952ee82 144 u32 = y31;
RichellBooyink 0:54c54952ee82 145 y32 = biquad (u32, dno1_v1, dno1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
RichellBooyink 0:54c54952ee82 146 u33 = y32;
RichellBooyink 0:54c54952ee82 147 y33 = biquad (u33, dno2_v1, dno2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
RichellBooyink 0:54c54952ee82 148 u34 = y33;
RichellBooyink 0:54c54952ee82 149 y34 = biquad (u34, dno3_v1, dno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
RichellBooyink 0:54c54952ee82 150 // Rectify sample
RichellBooyink 0:54c54952ee82 151 y35 = fabs(y34);
RichellBooyink 0:54c54952ee82 152 // Make it smooth
RichellBooyink 0:54c54952ee82 153 u35 = y35;
RichellBooyink 0:54c54952ee82 154 y36 = biquad (u35, dlp1_v1, dlp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1*0.065187, flp1_b2*0.065187);
RichellBooyink 0:54c54952ee82 155 u36 = y36;
RichellBooyink 0:54c54952ee82 156 final_filter4 = biquad(u36, dlp2_v1, dlp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);
RichellBooyink 0:54c54952ee82 157
RichellBooyink 0:54c54952ee82 158 scope.set (0, final_filter1);
RichellBooyink 0:54c54952ee82 159 scope.set (1,final_filter2);
RichellBooyink 0:54c54952ee82 160 scope.set (2,final_filter3);
RichellBooyink 0:54c54952ee82 161 scope.set (3,final_filter4);
RichellBooyink 0:54c54952ee82 162 scope.send ();
RichellBooyink 0:54c54952ee82 163 }
RichellBooyink 0:54c54952ee82 164
RichellBooyink 0:54c54952ee82 165 int main ()
RichellBooyink 0:54c54952ee82 166 {
RichellBooyink 0:54c54952ee82 167 EMG_Control.attach_us(MyController,5e4);
RichellBooyink 0:54c54952ee82 168 }