For BIOROBOTICS PROJECT
Dependencies: HIDScope MODSERIAL QEI mbed
main.cpp@2:ac98d055a6cd, 2015-10-28 (annotated)
- Committer:
- dbayuadi
- Date:
- Wed Oct 28 09:47:23 2015 +0000
- Revision:
- 2:ac98d055a6cd
- Parent:
- 1:3bae5ab25e20
filter EMG
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
dbayuadi | 0:8ccd4c66e07f | 1 | #include "mbed.h" |
dbayuadi | 0:8ccd4c66e07f | 2 | #include "HIDScope.h" |
dbayuadi | 0:8ccd4c66e07f | 3 | #include "QEI.h" |
dbayuadi | 2:ac98d055a6cd | 4 | AnalogIn emg_left(A0); |
dbayuadi | 2:ac98d055a6cd | 5 | AnalogIn emg_right(A1); |
dbayuadi | 2:ac98d055a6cd | 6 | DigitalOut led(LED_BLUE); |
dbayuadi | 2:ac98d055a6cd | 7 | HIDScope scope(6); |
dbayuadi | 0:8ccd4c66e07f | 8 | Ticker get; |
dbayuadi | 0:8ccd4c66e07f | 9 | AnalogIn pot(A1); |
dbayuadi | 0:8ccd4c66e07f | 10 | Serial pc(USBTX, USBRX); // tx, rx |
dbayuadi | 0:8ccd4c66e07f | 11 | volatile bool fn_go = false; |
dbayuadi | 0:8ccd4c66e07f | 12 | double y_notch_1; |
dbayuadi | 0:8ccd4c66e07f | 13 | double y_notch_2; |
dbayuadi | 0:8ccd4c66e07f | 14 | double y_high; |
dbayuadi | 0:8ccd4c66e07f | 15 | double y_low; |
dbayuadi | 1:3bae5ab25e20 | 16 | double y_rect; |
dbayuadi | 2:ac98d055a6cd | 17 | const double Fs = 500*2; |
dbayuadi | 2:ac98d055a6cd | 18 | const double Ts = 1/Fs; |
dbayuadi | 1:3bae5ab25e20 | 19 | double a; |
dbayuadi | 2:ac98d055a6cd | 20 | const double gain_notch_1 = 1.000000; |
dbayuadi | 0:8ccd4c66e07f | 21 | const double b0_notch_1 = 1.0*gain_notch_1; |
dbayuadi | 2:ac98d055a6cd | 22 | const double b1_notch_1 = -1.90215058158*gain_notch_1; |
dbayuadi | 0:8ccd4c66e07f | 23 | const double b2_notch_1 = 1.0*gain_notch_1; |
dbayuadi | 2:ac98d055a6cd | 24 | const double a1_notch_1 = -1.76851639902; |
dbayuadi | 2:ac98d055a6cd | 25 | const double a2_notch_1 = 0.89458928499; |
dbayuadi | 0:8ccd4c66e07f | 26 | double v1_notch_1 = 0, v2_notch_1 = 0; |
dbayuadi | 0:8ccd4c66e07f | 27 | |
dbayuadi | 0:8ccd4c66e07f | 28 | |
dbayuadi | 0:8ccd4c66e07f | 29 | const double gain_notch_2 = 1.000000; |
dbayuadi | 0:8ccd4c66e07f | 30 | const double b0_notch_2 = 1.0*gain_notch_2; |
dbayuadi | 2:ac98d055a6cd | 31 | const double b1_notch_2 = -1.90215058158*gain_notch_2; |
dbayuadi | 0:8ccd4c66e07f | 32 | const double b2_notch_2 = 1.0*gain_notch_2; |
dbayuadi | 2:ac98d055a6cd | 33 | const double a1_notch_2 = -1.85196790295; |
dbayuadi | 2:ac98d055a6cd | 34 | const double a2_notch_2 = 0.92089778695; |
dbayuadi | 0:8ccd4c66e07f | 35 | double v1_notch_2 = 0, v2_notch_2 = 0; |
dbayuadi | 2:ac98d055a6cd | 36 | //high pass of 5 Hz Wc |
dbayuadi | 0:8ccd4c66e07f | 37 | |
dbayuadi | 2:ac98d055a6cd | 38 | const double gain_high = 1; |
dbayuadi | 2:ac98d055a6cd | 39 | const double b0_high = 0.978030479206560*gain_high; |
dbayuadi | 2:ac98d055a6cd | 40 | const double b1_high = -1.956060958413119*gain_high; |
dbayuadi | 2:ac98d055a6cd | 41 | const double b2_high = 0.978030479206560*gain_high; |
dbayuadi | 2:ac98d055a6cd | 42 | const double a1_high = -1.955578240315036; |
dbayuadi | 2:ac98d055a6cd | 43 | const double a2_high = 0.956543676511203; |
dbayuadi | 0:8ccd4c66e07f | 44 | double v1_high = 0, v2_high = 0; |
dbayuadi | 2:ac98d055a6cd | 45 | double v1_high_1 = 0, v2_high_1=0; |
dbayuadi | 0:8ccd4c66e07f | 46 | |
dbayuadi | 2:ac98d055a6cd | 47 | //low pass of 0.3 Hz Wc |
dbayuadi | 2:ac98d055a6cd | 48 | const double gain_low = 0.00001; |
dbayuadi | 2:ac98d055a6cd | 49 | const double b0_low = 0.088708177364838*gain_low; |
dbayuadi | 2:ac98d055a6cd | 50 | const double b1_low = 0.177416354729676*gain_low; |
dbayuadi | 2:ac98d055a6cd | 51 | const double b2_low = 0.088708177364838*gain_low; |
dbayuadi | 2:ac98d055a6cd | 52 | const double a1_low = -1.997334271812535; |
dbayuadi | 2:ac98d055a6cd | 53 | const double a2_low = 0.997337820139629; |
dbayuadi | 0:8ccd4c66e07f | 54 | double v1_low = 0, v2_low = 0; |
dbayuadi | 2:ac98d055a6cd | 55 | double v1_low_1 = 0,v2_low_1=0; |
dbayuadi | 0:8ccd4c66e07f | 56 | double biquad(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2) |
dbayuadi | 0:8ccd4c66e07f | 57 | { |
dbayuadi | 0:8ccd4c66e07f | 58 | double v = u - a1*v1 - a2*v2; |
dbayuadi | 0:8ccd4c66e07f | 59 | double y = b0*v + b1*v1 + b2*v2; |
dbayuadi | 0:8ccd4c66e07f | 60 | v2 = v1; |
dbayuadi | 0:8ccd4c66e07f | 61 | v1 = v; |
dbayuadi | 0:8ccd4c66e07f | 62 | return y; |
dbayuadi | 0:8ccd4c66e07f | 63 | } |
dbayuadi | 0:8ccd4c66e07f | 64 | |
dbayuadi | 2:ac98d055a6cd | 65 | double biquad1(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2) |
dbayuadi | 2:ac98d055a6cd | 66 | { |
dbayuadi | 2:ac98d055a6cd | 67 | double v = u - a1*v1 - a2*v2; |
dbayuadi | 2:ac98d055a6cd | 68 | double y = b0*v + b1*v1 + b2*v2; |
dbayuadi | 2:ac98d055a6cd | 69 | v2 = v1; |
dbayuadi | 2:ac98d055a6cd | 70 | v1 = v; |
dbayuadi | 2:ac98d055a6cd | 71 | return y; |
dbayuadi | 2:ac98d055a6cd | 72 | } |
dbayuadi | 2:ac98d055a6cd | 73 | |
dbayuadi | 2:ac98d055a6cd | 74 | double biquad2(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2) |
dbayuadi | 2:ac98d055a6cd | 75 | { |
dbayuadi | 2:ac98d055a6cd | 76 | double v = u - a1*v1 - a2*v2; |
dbayuadi | 2:ac98d055a6cd | 77 | double y = b0*v + b1*v1 + b2*v2; |
dbayuadi | 2:ac98d055a6cd | 78 | v2 = v1; |
dbayuadi | 2:ac98d055a6cd | 79 | v1 = v; |
dbayuadi | 2:ac98d055a6cd | 80 | return y; |
dbayuadi | 2:ac98d055a6cd | 81 | } |
dbayuadi | 2:ac98d055a6cd | 82 | |
dbayuadi | 2:ac98d055a6cd | 83 | double biquad3(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2) |
dbayuadi | 2:ac98d055a6cd | 84 | { |
dbayuadi | 2:ac98d055a6cd | 85 | double v = u - a1*v1 - a2*v2; |
dbayuadi | 2:ac98d055a6cd | 86 | double y = b0*v + b1*v1 + b2*v2; |
dbayuadi | 2:ac98d055a6cd | 87 | v2 = v1; |
dbayuadi | 2:ac98d055a6cd | 88 | v1 = v; |
dbayuadi | 2:ac98d055a6cd | 89 | return y; |
dbayuadi | 2:ac98d055a6cd | 90 | } |
dbayuadi | 2:ac98d055a6cd | 91 | |
dbayuadi | 2:ac98d055a6cd | 92 | double biquad4(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2) |
dbayuadi | 2:ac98d055a6cd | 93 | { |
dbayuadi | 2:ac98d055a6cd | 94 | double v = u - a1*v1 - a2*v2; |
dbayuadi | 2:ac98d055a6cd | 95 | double y = b0*v + b1*v1 + b2*v2; |
dbayuadi | 2:ac98d055a6cd | 96 | v2 = v1; |
dbayuadi | 2:ac98d055a6cd | 97 | v1 = v; |
dbayuadi | 2:ac98d055a6cd | 98 | return y; |
dbayuadi | 2:ac98d055a6cd | 99 | } |
dbayuadi | 2:ac98d055a6cd | 100 | |
dbayuadi | 2:ac98d055a6cd | 101 | |
dbayuadi | 2:ac98d055a6cd | 102 | |
dbayuadi | 2:ac98d055a6cd | 103 | |
dbayuadi | 0:8ccd4c66e07f | 104 | void fn_activate() |
dbayuadi | 0:8ccd4c66e07f | 105 | { |
dbayuadi | 0:8ccd4c66e07f | 106 | |
dbayuadi | 0:8ccd4c66e07f | 107 | fn_go = true; |
dbayuadi | 0:8ccd4c66e07f | 108 | } |
dbayuadi | 0:8ccd4c66e07f | 109 | |
dbayuadi | 2:ac98d055a6cd | 110 | double z1; |
dbayuadi | 2:ac98d055a6cd | 111 | double y_high_1; |
dbayuadi | 2:ac98d055a6cd | 112 | double y_notch_1_1; |
dbayuadi | 2:ac98d055a6cd | 113 | double y_notch_2_1; |
dbayuadi | 2:ac98d055a6cd | 114 | double y_rect_1; |
dbayuadi | 2:ac98d055a6cd | 115 | double y_low_1; |
dbayuadi | 2:ac98d055a6cd | 116 | double t = 0; |
dbayuadi | 2:ac98d055a6cd | 117 | double value = 0; |
dbayuadi | 2:ac98d055a6cd | 118 | double max() // calibration |
dbayuadi | 1:3bae5ab25e20 | 119 | { |
dbayuadi | 2:ac98d055a6cd | 120 | if (t <= 10) { |
dbayuadi | 2:ac98d055a6cd | 121 | value = value + 0.0; |
dbayuadi | 2:ac98d055a6cd | 122 | } else if(t>10 && t<=20) { |
dbayuadi | 2:ac98d055a6cd | 123 | if(value<=y_low) { |
dbayuadi | 2:ac98d055a6cd | 124 | value = y_low; |
dbayuadi | 2:ac98d055a6cd | 125 | |
dbayuadi | 2:ac98d055a6cd | 126 | } else { |
dbayuadi | 2:ac98d055a6cd | 127 | value = value +0.0; |
dbayuadi | 1:3bae5ab25e20 | 128 | } |
dbayuadi | 2:ac98d055a6cd | 129 | |
dbayuadi | 2:ac98d055a6cd | 130 | } else { |
dbayuadi | 2:ac98d055a6cd | 131 | value = value + 0.0; |
dbayuadi | 2:ac98d055a6cd | 132 | |
dbayuadi | 2:ac98d055a6cd | 133 | |
dbayuadi | 2:ac98d055a6cd | 134 | } |
dbayuadi | 2:ac98d055a6cd | 135 | return value*0.80; |
dbayuadi | 1:3bae5ab25e20 | 136 | } |
dbayuadi | 2:ac98d055a6cd | 137 | double val; |
dbayuadi | 2:ac98d055a6cd | 138 | double gain() |
dbayuadi | 2:ac98d055a6cd | 139 | { |
dbayuadi | 2:ac98d055a6cd | 140 | |
dbayuadi | 2:ac98d055a6cd | 141 | if(max()==0) { |
dbayuadi | 2:ac98d055a6cd | 142 | val = 0; |
dbayuadi | 2:ac98d055a6cd | 143 | } else if(max()>=0) { |
dbayuadi | 2:ac98d055a6cd | 144 | val = 1/max(); |
dbayuadi | 2:ac98d055a6cd | 145 | } |
dbayuadi | 2:ac98d055a6cd | 146 | return val; |
dbayuadi | 2:ac98d055a6cd | 147 | } |
dbayuadi | 2:ac98d055a6cd | 148 | const double a1 = 11.1111; |
dbayuadi | 2:ac98d055a6cd | 149 | const double b1 = -0.1111; |
dbayuadi | 2:ac98d055a6cd | 150 | const double a2 = 4.7619; |
dbayuadi | 2:ac98d055a6cd | 151 | const double b2 = -0.047619; |
dbayuadi | 0:8ccd4c66e07f | 152 | void scopeSend() |
dbayuadi | 0:8ccd4c66e07f | 153 | { |
dbayuadi | 2:ac98d055a6cd | 154 | y_high = biquad1(emg_left.read(), v1_high, v2_high, a1_high, a2_high, b0_high, b1_high, b2_high); |
dbayuadi | 2:ac98d055a6cd | 155 | y_rect = abs(y_high); |
dbayuadi | 2:ac98d055a6cd | 156 | y_low = biquad2(y_rect, v1_low, v2_low, a1_low, a2_low, b0_low, b1_low, b2_low); |
dbayuadi | 2:ac98d055a6cd | 157 | t = t+Ts; |
dbayuadi | 2:ac98d055a6cd | 158 | y_high_1 = biquad3(emg_right.read(), v1_high_1, v2_high_1, a1_high, a2_high, b0_high, b1_high, b2_high); |
dbayuadi | 2:ac98d055a6cd | 159 | y_rect_1 = abs(y_high_1); |
dbayuadi | 2:ac98d055a6cd | 160 | y_low_1 = biquad4(y_rect_1, v1_low_1, v2_low_1, a1_low, a2_low, b0_low, b1_low, b2_low); |
dbayuadi | 2:ac98d055a6cd | 161 | |
dbayuadi | 2:ac98d055a6cd | 162 | scope.set(0,emg_left.read()); |
dbayuadi | 2:ac98d055a6cd | 163 | scope.set(1,emg_right.read()); |
dbayuadi | 2:ac98d055a6cd | 164 | scope.set(2,y_low); |
dbayuadi | 2:ac98d055a6cd | 165 | scope.set(3,y_low_1); |
dbayuadi | 2:ac98d055a6cd | 166 | scope.set(4,a1*y_low + b1); |
dbayuadi | 2:ac98d055a6cd | 167 | scope.set(5,a2*y_low_1 + b2); |
dbayuadi | 2:ac98d055a6cd | 168 | |
dbayuadi | 1:3bae5ab25e20 | 169 | |
dbayuadi | 0:8ccd4c66e07f | 170 | scope.send(); |
dbayuadi | 0:8ccd4c66e07f | 171 | } |
dbayuadi | 0:8ccd4c66e07f | 172 | |
dbayuadi | 0:8ccd4c66e07f | 173 | int main() |
dbayuadi | 0:8ccd4c66e07f | 174 | { |
dbayuadi | 2:ac98d055a6cd | 175 | get.attach(&fn_activate,Ts); |
dbayuadi | 0:8ccd4c66e07f | 176 | |
dbayuadi | 0:8ccd4c66e07f | 177 | while (true) { |
dbayuadi | 2:ac98d055a6cd | 178 | if(t<=10) { |
dbayuadi | 2:ac98d055a6cd | 179 | led = 0; |
dbayuadi | 2:ac98d055a6cd | 180 | } |
dbayuadi | 2:ac98d055a6cd | 181 | if(t>10 && t<=20) |
dbayuadi | 2:ac98d055a6cd | 182 | { |
dbayuadi | 2:ac98d055a6cd | 183 | led = 1; |
dbayuadi | 2:ac98d055a6cd | 184 | } |
dbayuadi | 2:ac98d055a6cd | 185 | if(t>20) |
dbayuadi | 2:ac98d055a6cd | 186 | { |
dbayuadi | 2:ac98d055a6cd | 187 | led = 0; |
dbayuadi | 2:ac98d055a6cd | 188 | } |
dbayuadi | 0:8ccd4c66e07f | 189 | if(fn_go == true) { |
dbayuadi | 2:ac98d055a6cd | 190 | |
dbayuadi | 0:8ccd4c66e07f | 191 | scopeSend(); |
dbayuadi | 2:ac98d055a6cd | 192 | |
dbayuadi | 2:ac98d055a6cd | 193 | |
dbayuadi | 0:8ccd4c66e07f | 194 | fn_go = false; |
dbayuadi | 0:8ccd4c66e07f | 195 | } |
dbayuadi | 0:8ccd4c66e07f | 196 | } |
dbayuadi | 0:8ccd4c66e07f | 197 | } |