Richell Booyink
/
EMG_TEST
4 EMG filters die werken en op HIDScope afgebeeld kunnen worden
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?
User | Revision | Line number | New 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 | } |