EMG controlling calibration + filters biorobotics group 13 BMT
Dependencies: HIDScope biquadFilter mbed
main.cpp@7:d6416ed1672c, 2015-10-20 (annotated)
- Committer:
- RichellBooyink
- Date:
- Tue Oct 20 12:45:00 2015 +0000
- Revision:
- 7:d6416ed1672c
- Parent:
- 6:30698d9be7f8
- Child:
- 8:63a7b61ca2ae
Filters including calibrations works ;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
RichellBooyink | 0:2ded24549b70 | 1 | #include "mbed.h" |
RichellBooyink | 0:2ded24549b70 | 2 | #include "HIDScope.h" |
RichellBooyink | 0:2ded24549b70 | 3 | |
RichellBooyink | 4:8db85182a00d | 4 | AnalogIn EMG_bicepsleft(A0); |
RichellBooyink | 4:8db85182a00d | 5 | AnalogIn EMG_bicepsright (A1); |
RichellBooyink | 4:8db85182a00d | 6 | AnalogIn EMG_legleft (A2); |
RichellBooyink | 6:30698d9be7f8 | 7 | AnalogIn EMG_legright (A3); |
RichellBooyink | 3:61f0fc41f3bc | 8 | HIDScope scope(4); |
RichellBooyink | 6:30698d9be7f8 | 9 | Timer cali; |
RichellBooyink | 7:d6416ed1672c | 10 | Ticker filter; |
RichellBooyink | 0:2ded24549b70 | 11 | |
RichellBooyink | 0:2ded24549b70 | 12 | // Filter1 = High pass filter tot 20 Hz |
RichellBooyink | 0:2ded24549b70 | 13 | double fh1_v1=0, fh1_v2=0, fh2_v1=0, fh2_v2=0; |
RichellBooyink | 0:2ded24549b70 | 14 | const double fh1_a1=-0.84909054461, fh1_a2=0.00000000000, fh1_b0= 1, fh1_b1=-1, fh1_b2=0; |
RichellBooyink | 0:2ded24549b70 | 15 | const double fh2_a1=-1.82553264091, fh2_a2=0.85001416809, fh2_b0= 1, fh2_b1=-2, fh2_b2=1; |
RichellBooyink | 0:2ded24549b70 | 16 | // Filter2 = Low pass filter na 60 Hz |
RichellBooyink | 0:2ded24549b70 | 17 | double fl1_v1=0, fl1_v2=0, fl2_v1=0, fl2_v2=0; |
RichellBooyink | 0:2ded24549b70 | 18 | const double fl1_a1=-0.66979455390, fl1_a2=0.00000000000, fl1_b0= 1, fl1_b1=1, fl1_b2=0; |
RichellBooyink | 0:2ded24549b70 | 19 | const double fl2_a1=-1.55376616139, fl2_a2=0.68023470431, fl2_b0= 1, fl2_b1=2, fl2_b2=1; |
RichellBooyink | 0:2ded24549b70 | 20 | // Filter3 = Notch filter at 50 Hz |
RichellBooyink | 0:2ded24549b70 | 21 | double fno1_v1=0, fno1_v2=0, fno2_v1=0, fno2_v2=0, fno3_v1=0, fno3_v2=0; |
RichellBooyink | 0:2ded24549b70 | 22 | const double fno1_a1 = -1.87934916386, fno1_a2= 0.97731851355, fno1_b0= 1, fno1_b1= -1.90090686046, fno1_b2= 1; |
RichellBooyink | 0:2ded24549b70 | 23 | const double fno2_a1 = -1.88341028603, fno2_a2= 0.98825147717, fno2_b0= 1, fno2_b1= -1.90090686046, fno2_b2= 1; |
RichellBooyink | 0:2ded24549b70 | 24 | const double fno3_a1 = -1.89635403726, fno3_a2= 0.98894004849, fno3_b0= 1, fno3_b1= -1.90090686046, fno3_b2= 1; |
RichellBooyink | 4:8db85182a00d | 25 | // Filter4 = Lowpass filter at 5 Hz |
RichellBooyink | 4:8db85182a00d | 26 | double flp1_v1=0, flp1_v2=0, flp2_v1=0, flp2_v2=0; |
RichellBooyink | 4:8db85182a00d | 27 | const double flp1_a1=-0.97922725527, flp1_a2=0.00000000000, flp1_b0= 1, flp1_b1=1, flp1_b2=0; |
RichellBooyink | 4:8db85182a00d | 28 | const double flp2_a1=-1.97879353121, flp2_a2=0.97922951943, flp2_b0= 1, flp2_b1=2, flp2_b2=1; |
RichellBooyink | 6:30698d9be7f8 | 29 | double y1, y2, y3, y4, y5, y6, y7, y8, y9, u1, u2, u3, u4, u5, u6, u7,u8 , u9 ; |
RichellBooyink | 6:30698d9be7f8 | 30 | double final_filter1, final_filter2, final_filter3, final_filter4; |
RichellBooyink | 6:30698d9be7f8 | 31 | double fblmax = 0; |
RichellBooyink | 6:30698d9be7f8 | 32 | double fbrmax = 0; |
RichellBooyink | 6:30698d9be7f8 | 33 | double fllmax = 0; |
RichellBooyink | 6:30698d9be7f8 | 34 | double flrmax = 0; |
RichellBooyink | 6:30698d9be7f8 | 35 | |
RichellBooyink | 6:30698d9be7f8 | 36 | void Calibrations () |
RichellBooyink | 6:30698d9be7f8 | 37 | { |
RichellBooyink | 6:30698d9be7f8 | 38 | cali.start(); |
RichellBooyink | 6:30698d9be7f8 | 39 | while (cali.read()<= 5) { |
RichellBooyink | 6:30698d9be7f8 | 40 | if (final_filter1 >= fblmax) { |
RichellBooyink | 6:30698d9be7f8 | 41 | fblmax=final_filter1; |
RichellBooyink | 6:30698d9be7f8 | 42 | } |
RichellBooyink | 6:30698d9be7f8 | 43 | if (final_filter2 >= fbrmax) { |
RichellBooyink | 6:30698d9be7f8 | 44 | fbrmax = final_filter2; |
RichellBooyink | 6:30698d9be7f8 | 45 | } |
RichellBooyink | 6:30698d9be7f8 | 46 | if (final_filter3 >= fllmax) { |
RichellBooyink | 6:30698d9be7f8 | 47 | fllmax=final_filter3; |
RichellBooyink | 6:30698d9be7f8 | 48 | } |
RichellBooyink | 6:30698d9be7f8 | 49 | if (final_filter4 >= flrmax) { |
RichellBooyink | 6:30698d9be7f8 | 50 | flrmax=final_filter4; |
RichellBooyink | 6:30698d9be7f8 | 51 | } |
RichellBooyink | 6:30698d9be7f8 | 52 | } |
RichellBooyink | 6:30698d9be7f8 | 53 | } |
RichellBooyink | 6:30698d9be7f8 | 54 | |
RichellBooyink | 3:61f0fc41f3bc | 55 | |
RichellBooyink | 3:61f0fc41f3bc | 56 | // Standaard formule voor het biquad filter |
RichellBooyink | 3:61f0fc41f3bc | 57 | double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2) |
RichellBooyink | 3:61f0fc41f3bc | 58 | |
RichellBooyink | 6:30698d9be7f8 | 59 | { |
RichellBooyink | 3:61f0fc41f3bc | 60 | double v = u - a1*v1 - a2*v2; |
RichellBooyink | 3:61f0fc41f3bc | 61 | double y = b0*v + b1*v1 + b2*v2; |
RichellBooyink | 3:61f0fc41f3bc | 62 | v2=v1; |
RichellBooyink | 3:61f0fc41f3bc | 63 | v1=v; |
RichellBooyink | 3:61f0fc41f3bc | 64 | return y; |
RichellBooyink | 3:61f0fc41f3bc | 65 | } |
RichellBooyink | 3:61f0fc41f3bc | 66 | |
RichellBooyink | 5:e78295b978ab | 67 | void Filters_bicepsleft() |
RichellBooyink | 0:2ded24549b70 | 68 | { |
RichellBooyink | 4:8db85182a00d | 69 | u1 = EMG_bicepsleft.read(); |
RichellBooyink | 2:e3259334299e | 70 | //Highpass |
RichellBooyink | 3:61f0fc41f3bc | 71 | y1 = biquad (u1, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547); |
RichellBooyink | 3:61f0fc41f3bc | 72 | u2 = y1; |
RichellBooyink | 3:61f0fc41f3bc | 73 | y2 = biquad (u2, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885); |
RichellBooyink | 2:e3259334299e | 74 | //Lowpass |
RichellBooyink | 2:e3259334299e | 75 | u3 = y2; |
RichellBooyink | 3:61f0fc41f3bc | 76 | y3 = biquad (u3, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103); |
RichellBooyink | 2:e3259334299e | 77 | u4 = y3; |
RichellBooyink | 3:61f0fc41f3bc | 78 | y4 = biquad (u4, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617); |
RichellBooyink | 2:e3259334299e | 79 | // Notch |
RichellBooyink | 2:e3259334299e | 80 | u5 = y4; |
RichellBooyink | 3:61f0fc41f3bc | 81 | y5 = biquad (u5, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206); |
RichellBooyink | 2:e3259334299e | 82 | u6 = y5; |
RichellBooyink | 3:61f0fc41f3bc | 83 | y6 = biquad (u6, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2); |
RichellBooyink | 2:e3259334299e | 84 | u7 = y6; |
RichellBooyink | 3:61f0fc41f3bc | 85 | y7 = biquad (u7, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227); |
RichellBooyink | 6:30698d9be7f8 | 86 | // Rectify sample |
RichellBooyink | 4:8db85182a00d | 87 | y8 = fabs(y7); |
RichellBooyink | 6:30698d9be7f8 | 88 | // Make it smooth |
RichellBooyink | 6:30698d9be7f8 | 89 | u8 = y8; |
RichellBooyink | 4:8db85182a00d | 90 | y9 = biquad (u8, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386); |
RichellBooyink | 4:8db85182a00d | 91 | u9 = y9; |
RichellBooyink | 6:30698d9be7f8 | 92 | final_filter1 = biquad(u9, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109); |
RichellBooyink | 6:30698d9be7f8 | 93 | } |
RichellBooyink | 6:30698d9be7f8 | 94 | |
RichellBooyink | 6:30698d9be7f8 | 95 | void Filters_bicepsright() |
RichellBooyink | 5:e78295b978ab | 96 | { |
RichellBooyink | 5:e78295b978ab | 97 | u1 = EMG_bicepsright.read(); |
RichellBooyink | 5:e78295b978ab | 98 | //Highpass |
RichellBooyink | 5:e78295b978ab | 99 | y1 = biquad (u1, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547); |
RichellBooyink | 5:e78295b978ab | 100 | u2 = y1; |
RichellBooyink | 5:e78295b978ab | 101 | y2 = biquad (u2, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885); |
RichellBooyink | 5:e78295b978ab | 102 | //Lowpass |
RichellBooyink | 5:e78295b978ab | 103 | u3 = y2; |
RichellBooyink | 5:e78295b978ab | 104 | y3 = biquad (u3, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103); |
RichellBooyink | 5:e78295b978ab | 105 | u4 = y3; |
RichellBooyink | 5:e78295b978ab | 106 | y4 = biquad (u4, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617); |
RichellBooyink | 5:e78295b978ab | 107 | // Notch |
RichellBooyink | 5:e78295b978ab | 108 | u5 = y4; |
RichellBooyink | 5:e78295b978ab | 109 | y5 = biquad (u5, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206); |
RichellBooyink | 5:e78295b978ab | 110 | u6 = y5; |
RichellBooyink | 5:e78295b978ab | 111 | y6 = biquad (u6, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2); |
RichellBooyink | 5:e78295b978ab | 112 | u7 = y6; |
RichellBooyink | 5:e78295b978ab | 113 | y7 = biquad (u7, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227); |
RichellBooyink | 6:30698d9be7f8 | 114 | // Rectify sample |
RichellBooyink | 5:e78295b978ab | 115 | y8 = fabs(y7); |
RichellBooyink | 6:30698d9be7f8 | 116 | // Make it smooth |
RichellBooyink | 6:30698d9be7f8 | 117 | u8 = y8; |
RichellBooyink | 5:e78295b978ab | 118 | y9 = biquad (u8, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386); |
RichellBooyink | 5:e78295b978ab | 119 | u9 = y9; |
RichellBooyink | 6:30698d9be7f8 | 120 | final_filter2 = biquad(u9, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109); |
RichellBooyink | 6:30698d9be7f8 | 121 | } |
RichellBooyink | 6:30698d9be7f8 | 122 | void Filters_legleft() |
RichellBooyink | 5:e78295b978ab | 123 | { |
RichellBooyink | 5:e78295b978ab | 124 | u1 = EMG_legleft.read(); |
RichellBooyink | 5:e78295b978ab | 125 | //Highpass |
RichellBooyink | 5:e78295b978ab | 126 | y1 = biquad (u1, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547); |
RichellBooyink | 5:e78295b978ab | 127 | u2 = y1; |
RichellBooyink | 5:e78295b978ab | 128 | y2 = biquad (u2, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885); |
RichellBooyink | 5:e78295b978ab | 129 | //Lowpass |
RichellBooyink | 5:e78295b978ab | 130 | u3 = y2; |
RichellBooyink | 5:e78295b978ab | 131 | y3 = biquad (u3, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103); |
RichellBooyink | 5:e78295b978ab | 132 | u4 = y3; |
RichellBooyink | 5:e78295b978ab | 133 | y4 = biquad (u4, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617); |
RichellBooyink | 5:e78295b978ab | 134 | // Notch |
RichellBooyink | 5:e78295b978ab | 135 | u5 = y4; |
RichellBooyink | 5:e78295b978ab | 136 | y5 = biquad (u5, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206); |
RichellBooyink | 5:e78295b978ab | 137 | u6 = y5; |
RichellBooyink | 5:e78295b978ab | 138 | y6 = biquad (u6, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2); |
RichellBooyink | 5:e78295b978ab | 139 | u7 = y6; |
RichellBooyink | 5:e78295b978ab | 140 | y7 = biquad (u7, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227); |
RichellBooyink | 6:30698d9be7f8 | 141 | // Rectify sample |
RichellBooyink | 5:e78295b978ab | 142 | y8 = fabs(y7); |
RichellBooyink | 6:30698d9be7f8 | 143 | // Make it smooth |
RichellBooyink | 6:30698d9be7f8 | 144 | u8 = y8; |
RichellBooyink | 5:e78295b978ab | 145 | y9 = biquad (u8, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386); |
RichellBooyink | 5:e78295b978ab | 146 | u9 = y9; |
RichellBooyink | 6:30698d9be7f8 | 147 | final_filter3 = biquad(u9, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109); |
RichellBooyink | 6:30698d9be7f8 | 148 | } |
RichellBooyink | 6:30698d9be7f8 | 149 | void Filters_legright() |
RichellBooyink | 5:e78295b978ab | 150 | { |
RichellBooyink | 5:e78295b978ab | 151 | u1 = EMG_legright.read(); |
RichellBooyink | 5:e78295b978ab | 152 | //Highpass |
RichellBooyink | 5:e78295b978ab | 153 | y1 = biquad (u1, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547); |
RichellBooyink | 5:e78295b978ab | 154 | u2 = y1; |
RichellBooyink | 5:e78295b978ab | 155 | y2 = biquad (u2, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885); |
RichellBooyink | 5:e78295b978ab | 156 | //Lowpass |
RichellBooyink | 5:e78295b978ab | 157 | u3 = y2; |
RichellBooyink | 5:e78295b978ab | 158 | y3 = biquad (u3, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103); |
RichellBooyink | 5:e78295b978ab | 159 | u4 = y3; |
RichellBooyink | 5:e78295b978ab | 160 | y4 = biquad (u4, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617); |
RichellBooyink | 5:e78295b978ab | 161 | // Notch |
RichellBooyink | 5:e78295b978ab | 162 | u5 = y4; |
RichellBooyink | 5:e78295b978ab | 163 | y5 = biquad (u5, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206); |
RichellBooyink | 5:e78295b978ab | 164 | u6 = y5; |
RichellBooyink | 5:e78295b978ab | 165 | y6 = biquad (u6, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2); |
RichellBooyink | 5:e78295b978ab | 166 | u7 = y6; |
RichellBooyink | 5:e78295b978ab | 167 | y7 = biquad (u7, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227); |
RichellBooyink | 6:30698d9be7f8 | 168 | // Rectify sample |
RichellBooyink | 5:e78295b978ab | 169 | y8 = fabs(y7); |
RichellBooyink | 6:30698d9be7f8 | 170 | // Make it smooth |
RichellBooyink | 6:30698d9be7f8 | 171 | u8 = y8; |
RichellBooyink | 5:e78295b978ab | 172 | y9 = biquad (u8, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386); |
RichellBooyink | 5:e78295b978ab | 173 | u9 = y9; |
RichellBooyink | 6:30698d9be7f8 | 174 | final_filter4 = biquad(u9, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109); |
RichellBooyink | 6:30698d9be7f8 | 175 | } |
RichellBooyink | 5:e78295b978ab | 176 | // Send signals to HIDScope |
RichellBooyink | 6:30698d9be7f8 | 177 | void Filters_EMG () |
RichellBooyink | 6:30698d9be7f8 | 178 | { |
RichellBooyink | 6:30698d9be7f8 | 179 | scope.set (0,final_filter1); |
RichellBooyink | 6:30698d9be7f8 | 180 | scope.set (1,final_filter2); |
RichellBooyink | 6:30698d9be7f8 | 181 | scope.set (2,final_filter3); |
RichellBooyink | 6:30698d9be7f8 | 182 | scope.set (3,final_filter4); |
RichellBooyink | 1:16165e207e70 | 183 | scope.send (); |
RichellBooyink | 6:30698d9be7f8 | 184 | } |
RichellBooyink | 6:30698d9be7f8 | 185 | |
RichellBooyink | 0:2ded24549b70 | 186 | int main () |
RichellBooyink | 6:30698d9be7f8 | 187 | { |
RichellBooyink | 7:d6416ed1672c | 188 | Calibrations(); |
RichellBooyink | 6:30698d9be7f8 | 189 | filter.attach_us(Filters_EMG,1e3); |
RichellBooyink | 6:30698d9be7f8 | 190 | } |
RichellBooyink | 0:2ded24549b70 | 191 | |
RichellBooyink | 0:2ded24549b70 | 192 | |
RichellBooyink | 0:2ded24549b70 | 193 | |
RichellBooyink | 2:e3259334299e | 194 | |
RichellBooyink | 2:e3259334299e | 195 |