emg2
Dependencies: HIDScope biquadFilter mbed QEI
Fork of EMG by
main.cpp@30:14a9358758d1, 2018-10-31 (annotated)
- Committer:
- keeswieriks
- Date:
- Wed Oct 31 14:01:11 2018 +0000
- Revision:
- 30:14a9358758d1
- Parent:
- 29:9010b7bdbfbb
- Child:
- 31:c22110c9dca4
met uitlijning er ook bij;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vsluiter | 0:32bb76391d89 | 1 | #include "mbed.h" |
vsluiter | 11:ce72ec658a95 | 2 | #include "HIDScope.h" |
keeswieriks | 21:931fe86dbf5a | 3 | #include "BiQuad.h" |
keeswieriks | 29:9010b7bdbfbb | 4 | #include "QEI.h" |
vsluiter | 0:32bb76391d89 | 5 | |
s1574396 | 25:02f183b944ed | 6 | HIDScope scope( 6 ); |
s1574396 | 22:bcfee9594007 | 7 | Ticker sample_timer; |
s1574396 | 22:bcfee9594007 | 8 | |
keeswieriks | 29:9010b7bdbfbb | 9 | |
keeswieriks | 29:9010b7bdbfbb | 10 | Serial pc(USBTX,USBRX); |
keeswieriks | 29:9010b7bdbfbb | 11 | |
s1574396 | 23:dec549767006 | 12 | // Inputs EMG |
keeswieriks | 21:931fe86dbf5a | 13 | AnalogIn emg0_in( A0 ); |
keeswieriks | 21:931fe86dbf5a | 14 | AnalogIn emg1_in( A1 ); |
keeswieriks | 21:931fe86dbf5a | 15 | AnalogIn emg2_in( A2 ); |
keeswieriks | 21:931fe86dbf5a | 16 | |
keeswieriks | 29:9010b7bdbfbb | 17 | InterruptIn but1(SW2); |
keeswieriks | 29:9010b7bdbfbb | 18 | InterruptIn but2(SW3); |
keeswieriks | 29:9010b7bdbfbb | 19 | |
keeswieriks | 28:433d12c52913 | 20 | DigitalOut led1(LED_BLUE); |
keeswieriks | 28:433d12c52913 | 21 | DigitalOut led2(LED_RED); |
keeswieriks | 28:433d12c52913 | 22 | DigitalOut led3(LED_GREEN); |
keeswieriks | 28:433d12c52913 | 23 | |
s1574396 | 23:dec549767006 | 24 | // Constants EMG |
s1574396 | 23:dec549767006 | 25 | const double m1 = 0.5000; |
s1574396 | 23:dec549767006 | 26 | const double m2 = -0.8090; |
s1574396 | 23:dec549767006 | 27 | const double n0 = 0.5000; |
s1574396 | 23:dec549767006 | 28 | const double n1 = -0.8090; |
s1574396 | 23:dec549767006 | 29 | const double n2 = 0; |
s1574396 | 23:dec549767006 | 30 | const double a1 = 0.9565; |
s1574396 | 23:dec549767006 | 31 | const double a2 = -1.9131; |
s1574396 | 23:dec549767006 | 32 | const double b0 = 0.9565; |
s1574396 | 23:dec549767006 | 33 | const double b1 = -1.9112; |
s1574396 | 23:dec549767006 | 34 | const double b2 = 0.9150; |
s1574396 | 23:dec549767006 | 35 | const double c1 = 0.0675; |
s1574396 | 23:dec549767006 | 36 | const double c2 = 0.1349; |
s1574396 | 23:dec549767006 | 37 | const double d0 = 0.0675; |
s1574396 | 23:dec549767006 | 38 | const double d1 = -1.1430; |
s1574396 | 23:dec549767006 | 39 | const double d2 = 0.4128; |
s1574396 | 24:6bdc50e21805 | 40 | |
s1574396 | 23:dec549767006 | 41 | // Variables EMG |
s1574396 | 24:6bdc50e21805 | 42 | double emg0; |
s1574396 | 24:6bdc50e21805 | 43 | double emg1; |
s1574396 | 24:6bdc50e21805 | 44 | double emg2; |
s1574396 | 24:6bdc50e21805 | 45 | double notch0; |
s1574396 | 24:6bdc50e21805 | 46 | double notch1; |
s1574396 | 24:6bdc50e21805 | 47 | double notch2; |
s1574396 | 24:6bdc50e21805 | 48 | double high0; |
s1574396 | 24:6bdc50e21805 | 49 | double high1; |
s1574396 | 24:6bdc50e21805 | 50 | double high2; |
s1574396 | 24:6bdc50e21805 | 51 | double absolute0; |
s1574396 | 24:6bdc50e21805 | 52 | double absolute1; |
s1574396 | 24:6bdc50e21805 | 53 | double absolute2; |
s1574396 | 24:6bdc50e21805 | 54 | double low0; |
s1574396 | 24:6bdc50e21805 | 55 | double low1; |
s1574396 | 24:6bdc50e21805 | 56 | double low2; |
keeswieriks | 21:931fe86dbf5a | 57 | |
keeswieriks | 21:931fe86dbf5a | 58 | // BiQuad values |
keeswieriks | 21:931fe86dbf5a | 59 | BiQuadChain notch; |
keeswieriks | 21:931fe86dbf5a | 60 | BiQuad N1( m1, m2, n0, n1, n2); |
keeswieriks | 21:931fe86dbf5a | 61 | BiQuad N2( m1, m2, n0, n1, n2); |
keeswieriks | 21:931fe86dbf5a | 62 | BiQuad N3( m1, m2, n0, n1, n2); |
keeswieriks | 21:931fe86dbf5a | 63 | BiQuadChain highpass; |
keeswieriks | 21:931fe86dbf5a | 64 | BiQuad H1( a1, a2, b0, b1, b2); |
keeswieriks | 21:931fe86dbf5a | 65 | BiQuad H2( a1, a2, b0, b1, b2); |
keeswieriks | 21:931fe86dbf5a | 66 | BiQuad H3( a1, a2, b0, b1, b2); |
keeswieriks | 21:931fe86dbf5a | 67 | BiQuadChain lowpass; |
keeswieriks | 21:931fe86dbf5a | 68 | BiQuad L1( c1, c2, d0, d1, d2); |
keeswieriks | 21:931fe86dbf5a | 69 | BiQuad L2( c1, c2, d0, d1, d2); |
keeswieriks | 21:931fe86dbf5a | 70 | BiQuad L3( c1, c2, d0, d1, d2); |
keeswieriks | 21:931fe86dbf5a | 71 | |
keeswieriks | 29:9010b7bdbfbb | 72 | const float T=0.001f; |
keeswieriks | 29:9010b7bdbfbb | 73 | |
keeswieriks | 26:1eafb6111ae8 | 74 | // EMG |
keeswieriks | 27:6b4814ef266d | 75 | const int sizeMovAg = 100; //Size of array over which the moving average (MovAg) is calculated |
keeswieriks | 26:1eafb6111ae8 | 76 | double sum, sum1, sum2, sum3; //Variables used in calibration and MovAg to sum the elements in the array |
keeswieriks | 26:1eafb6111ae8 | 77 | double StoreArray0[sizeMovAg] = {}, StoreArray1[sizeMovAg] = {}, StoreArray2[sizeMovAg] = {}; |
keeswieriks | 26:1eafb6111ae8 | 78 | |
keeswieriks | 26:1eafb6111ae8 | 79 | //Empty arrays to calculate MovAgs |
keeswieriks | 26:1eafb6111ae8 | 80 | double Average0, Average1, Average2; //Outcome of MovAg |
keeswieriks | 26:1eafb6111ae8 | 81 | const int sizeCali = 2000; //Size of array over which the Threshold will be calculated |
keeswieriks | 26:1eafb6111ae8 | 82 | double StoreCali0[sizeCali] = {}, StoreCali1[sizeCali] = {}, StoreCali2[sizeCali] = {}; |
keeswieriks | 26:1eafb6111ae8 | 83 | //Empty arrays to calculate means in calibration |
keeswieriks | 26:1eafb6111ae8 | 84 | |
keeswieriks | 26:1eafb6111ae8 | 85 | double Mean0, Mean1, Mean2; //Mean of maximum contraction, calculated in the calibration |
keeswieriks | 26:1eafb6111ae8 | 86 | double Threshold0 = 1, Threshold1 = 1, Threshold2 = 1; //Thresholds for muscles 0 to 2 |
keeswieriks | 26:1eafb6111ae8 | 87 | int g = 0; //Part of the switch void, where the current state can be changed |
keeswieriks | 26:1eafb6111ae8 | 88 | int emg_calib=0; //After calibration this value will be 1, enabling the |
keeswieriks | 26:1eafb6111ae8 | 89 | |
keeswieriks | 26:1eafb6111ae8 | 90 | //EMG |
keeswieriks | 26:1eafb6111ae8 | 91 | Ticker Filter_tick; |
keeswieriks | 26:1eafb6111ae8 | 92 | Ticker MovAg_tick; |
keeswieriks | 26:1eafb6111ae8 | 93 | |
s1574396 | 22:bcfee9594007 | 94 | // Filter of the first EMG signal |
s1574396 | 24:6bdc50e21805 | 95 | void filtering() |
vsluiter | 2:e314bb3b2d99 | 96 | { |
s1574396 | 23:dec549767006 | 97 | emg0 = emg0_in.read(); // Reading the EMG signal |
s1574396 | 24:6bdc50e21805 | 98 | emg1 = emg1_in.read(); |
s1574396 | 24:6bdc50e21805 | 99 | emg2 = emg2_in.read(); |
s1574396 | 24:6bdc50e21805 | 100 | notch0 = N1.step(emg0); // Applying a notch filter over the EMG data |
s1574396 | 24:6bdc50e21805 | 101 | notch1 = N2.step(emg1); |
s1574396 | 24:6bdc50e21805 | 102 | notch2 = N3.step(emg2); |
s1574396 | 24:6bdc50e21805 | 103 | high0 = H1.step(notch0); // Applying a high pass filter |
s1574396 | 24:6bdc50e21805 | 104 | high1 = H2.step(notch1); |
s1574396 | 24:6bdc50e21805 | 105 | high2 = H3.step(notch2); |
s1574396 | 24:6bdc50e21805 | 106 | absolute0 = fabs(high0); // Rectifying the signal |
s1574396 | 24:6bdc50e21805 | 107 | absolute1 = fabs(high1); |
s1574396 | 24:6bdc50e21805 | 108 | absolute2 = fabs(high2); |
s1574396 | 24:6bdc50e21805 | 109 | low0 = L1.step(absolute0); // Applying low pass filter |
s1574396 | 24:6bdc50e21805 | 110 | low1 = L2.step(absolute1); |
s1574396 | 24:6bdc50e21805 | 111 | low2 = L3.step(absolute2); |
keeswieriks | 29:9010b7bdbfbb | 112 | } |
keeswieriks | 29:9010b7bdbfbb | 113 | |
keeswieriks | 29:9010b7bdbfbb | 114 | /*void EMG_filterd() |
keeswieriks | 29:9010b7bdbfbb | 115 | { |
keeswieriks | 29:9010b7bdbfbb | 116 | low0(); |
keeswieriks | 29:9010b7bdbfbb | 117 | low1(); |
keeswieriks | 29:9010b7bdbfbb | 118 | low2(); |
keeswieriks | 29:9010b7bdbfbb | 119 | }*/ |
keeswieriks | 29:9010b7bdbfbb | 120 | |
keeswieriks | 29:9010b7bdbfbb | 121 | void MovAg() |
keeswieriks | 29:9010b7bdbfbb | 122 | { |
keeswieriks | 26:1eafb6111ae8 | 123 | for (int i = sizeMovAg-1; i>=0; i--) { |
keeswieriks | 26:1eafb6111ae8 | 124 | //For statement to make an array of the last datapoints of the filtered signal |
keeswieriks | 26:1eafb6111ae8 | 125 | StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right |
keeswieriks | 26:1eafb6111ae8 | 126 | StoreArray1[i] = StoreArray1[i-1]; |
keeswieriks | 26:1eafb6111ae8 | 127 | StoreArray2[i] = StoreArray2[i-1]; |
keeswieriks | 26:1eafb6111ae8 | 128 | } |
keeswieriks | 26:1eafb6111ae8 | 129 | StoreArray0[0] = low0; //Stores the latest datapoint in the first element of the array |
keeswieriks | 26:1eafb6111ae8 | 130 | StoreArray1[0] = low1; |
keeswieriks | 26:1eafb6111ae8 | 131 | StoreArray2[0] = low2; |
keeswieriks | 26:1eafb6111ae8 | 132 | sum1 = 0.0; |
keeswieriks | 26:1eafb6111ae8 | 133 | sum2 = 0.0; |
keeswieriks | 26:1eafb6111ae8 | 134 | sum3 = 0.0; |
keeswieriks | 26:1eafb6111ae8 | 135 | for (int a = 0; a<=sizeMovAg-1; a++) { //For statement to sum the elements in the array |
keeswieriks | 26:1eafb6111ae8 | 136 | sum1+=StoreArray0[a]; |
keeswieriks | 26:1eafb6111ae8 | 137 | sum2+=StoreArray1[a]; |
keeswieriks | 26:1eafb6111ae8 | 138 | sum3+=StoreArray2[a]; |
keeswieriks | 26:1eafb6111ae8 | 139 | } |
keeswieriks | 26:1eafb6111ae8 | 140 | Average0 = sum1/sizeMovAg; //Calculates an average over the datapoints in the array |
keeswieriks | 26:1eafb6111ae8 | 141 | Average1 = sum2/sizeMovAg; |
keeswieriks | 26:1eafb6111ae8 | 142 | Average2 = sum3/sizeMovAg; |
keeswieriks | 26:1eafb6111ae8 | 143 | |
s1574396 | 25:02f183b944ed | 144 | scope.set( 0, emg0); // Sending the signal to the HIDScope |
s1574396 | 25:02f183b944ed | 145 | scope.set( 1, low0); // Change the numer of inputs on the top when necessary |
keeswieriks | 27:6b4814ef266d | 146 | scope.set( 2, Average0); |
s1574396 | 25:02f183b944ed | 147 | scope.set( 3, low1); |
s1574396 | 25:02f183b944ed | 148 | scope.set( 4, emg2); |
s1574396 | 25:02f183b944ed | 149 | scope.set( 5, low2); |
s1574396 | 24:6bdc50e21805 | 150 | scope.send(); |
keeswieriks | 21:931fe86dbf5a | 151 | } |
keeswieriks | 21:931fe86dbf5a | 152 | |
keeswieriks | 29:9010b7bdbfbb | 153 | void switch_to_calibrate() //Void to switch between signals to calibrate |
keeswieriks | 28:433d12c52913 | 154 | { |
keeswieriks | 28:433d12c52913 | 155 | g++; |
keeswieriks | 28:433d12c52913 | 156 | if (g == 0) |
keeswieriks | 28:433d12c52913 | 157 | { //If g = 0, led is blue |
keeswieriks | 28:433d12c52913 | 158 | led1=0; |
keeswieriks | 28:433d12c52913 | 159 | led2=1; |
keeswieriks | 28:433d12c52913 | 160 | led3=1; |
keeswieriks | 28:433d12c52913 | 161 | } |
keeswieriks | 28:433d12c52913 | 162 | else if(g == 1) |
keeswieriks | 28:433d12c52913 | 163 | { //If g = 1, led is red |
keeswieriks | 28:433d12c52913 | 164 | led1=1; |
keeswieriks | 28:433d12c52913 | 165 | led2=0; |
keeswieriks | 28:433d12c52913 | 166 | led3=1; |
keeswieriks | 28:433d12c52913 | 167 | |
keeswieriks | 28:433d12c52913 | 168 | } |
keeswieriks | 28:433d12c52913 | 169 | else if(g == 2) |
keeswieriks | 28:433d12c52913 | 170 | { //If g = 2, led is green |
keeswieriks | 28:433d12c52913 | 171 | led1=1; |
keeswieriks | 28:433d12c52913 | 172 | led2=1; |
keeswieriks | 28:433d12c52913 | 173 | led3=0; |
keeswieriks | 28:433d12c52913 | 174 | } |
keeswieriks | 28:433d12c52913 | 175 | else |
keeswieriks | 28:433d12c52913 | 176 | { //If g > 3, led is white |
keeswieriks | 28:433d12c52913 | 177 | led1=0; |
keeswieriks | 28:433d12c52913 | 178 | led2=0; |
keeswieriks | 28:433d12c52913 | 179 | led3=0; |
keeswieriks | 28:433d12c52913 | 180 | } |
keeswieriks | 28:433d12c52913 | 181 | } |
keeswieriks | 29:9010b7bdbfbb | 182 | void calibrate(void) //Void to calibrate the signals, depends on value g. While calibrating, maximal contraction is required |
keeswieriks | 26:1eafb6111ae8 | 183 | { |
keeswieriks | 30:14a9358758d1 | 184 | switch (g) |
keeswieriks | 30:14a9358758d1 | 185 | { |
keeswieriks | 30:14a9358758d1 | 186 | case 0: |
keeswieriks | 30:14a9358758d1 | 187 | { //Case zero, calibrate EMG signal of right biceps |
keeswieriks | 28:433d12c52913 | 188 | sum = 0.0; |
keeswieriks | 30:14a9358758d1 | 189 | for (int j = 0; j<=sizeCali-1; j++) |
keeswieriks | 30:14a9358758d1 | 190 | { |
keeswieriks | 28:433d12c52913 | 191 | //For statement to make an array of the latest datapoints of the filtered signal |
keeswieriks | 28:433d12c52913 | 192 | StoreCali0[j] = low0; //Stores the latest datapoint in the first element of the array |
keeswieriks | 28:433d12c52913 | 193 | sum+=StoreCali0[j]; //Sums the elements in the array |
keeswieriks | 28:433d12c52913 | 194 | wait(0.001f); |
keeswieriks | 28:433d12c52913 | 195 | } |
keeswieriks | 28:433d12c52913 | 196 | Mean0 = sum/sizeCali; //Calculates the mean of the signal |
keeswieriks | 29:9010b7bdbfbb | 197 | Threshold0 = Mean0; //Determines the threshold (factor 0.5) |
keeswieriks | 28:433d12c52913 | 198 | break; |
keeswieriks | 30:14a9358758d1 | 199 | } |
keeswieriks | 30:14a9358758d1 | 200 | case 1: |
keeswieriks | 30:14a9358758d1 | 201 | { //Case one, calibrate EMG signal of left biceps |
keeswieriks | 28:433d12c52913 | 202 | sum = 0.0; |
keeswieriks | 30:14a9358758d1 | 203 | for(int j=0; j<=sizeCali-1; j++) |
keeswieriks | 30:14a9358758d1 | 204 | { |
keeswieriks | 28:433d12c52913 | 205 | StoreCali1[j] = low1; |
keeswieriks | 28:433d12c52913 | 206 | sum+=StoreCali1[j]; |
keeswieriks | 28:433d12c52913 | 207 | wait(0.001f); |
keeswieriks | 28:433d12c52913 | 208 | } |
keeswieriks | 28:433d12c52913 | 209 | Mean1 = sum/sizeCali; |
keeswieriks | 29:9010b7bdbfbb | 210 | Threshold1 = Mean1; |
keeswieriks | 28:433d12c52913 | 211 | break; |
keeswieriks | 30:14a9358758d1 | 212 | } |
keeswieriks | 30:14a9358758d1 | 213 | case 2: |
keeswieriks | 30:14a9358758d1 | 214 | { //case two, calibrate EMG signal of calf |
keeswieriks | 28:433d12c52913 | 215 | sum = 0.0; |
keeswieriks | 30:14a9358758d1 | 216 | for(int j=0; j<=sizeCali-1; j++) |
keeswieriks | 30:14a9358758d1 | 217 | { |
keeswieriks | 28:433d12c52913 | 218 | StoreCali2[j] = low2; |
keeswieriks | 28:433d12c52913 | 219 | sum+=StoreCali2[j]; |
keeswieriks | 28:433d12c52913 | 220 | wait(0.001f); |
keeswieriks | 28:433d12c52913 | 221 | } |
keeswieriks | 28:433d12c52913 | 222 | Mean2 = sum/sizeCali; |
keeswieriks | 29:9010b7bdbfbb | 223 | Threshold2 = Mean2; |
keeswieriks | 28:433d12c52913 | 224 | break; |
keeswieriks | 30:14a9358758d1 | 225 | } |
keeswieriks | 30:14a9358758d1 | 226 | case 3: |
keeswieriks | 30:14a9358758d1 | 227 | { //Sets calibration value to 1; robot can be set to Home position |
keeswieriks | 28:433d12c52913 | 228 | emg_calib=1; |
keeswieriks | 28:433d12c52913 | 229 | wait(0.001f); |
keeswieriks | 28:433d12c52913 | 230 | break; |
keeswieriks | 30:14a9358758d1 | 231 | } |
keeswieriks | 30:14a9358758d1 | 232 | default: |
keeswieriks | 30:14a9358758d1 | 233 | { //Ensures nothing happens if g is not equal to 0,1 or 2. |
keeswieriks | 30:14a9358758d1 | 234 | break; |
keeswieriks | 30:14a9358758d1 | 235 | } |
keeswieriks | 30:14a9358758d1 | 236 | } |
keeswieriks | 29:9010b7bdbfbb | 237 | } |
keeswieriks | 29:9010b7bdbfbb | 238 | |
keeswieriks | 29:9010b7bdbfbb | 239 | |
keeswieriks | 29:9010b7bdbfbb | 240 | int main() |
keeswieriks | 29:9010b7bdbfbb | 241 | { |
keeswieriks | 29:9010b7bdbfbb | 242 | sample_timer.attach( &filtering, 0.002); |
keeswieriks | 29:9010b7bdbfbb | 243 | pc.baud(115200); // setting baudrate |
keeswieriks | 29:9010b7bdbfbb | 244 | |
keeswieriks | 29:9010b7bdbfbb | 245 | while(true) |
keeswieriks | 29:9010b7bdbfbb | 246 | { |
keeswieriks | 29:9010b7bdbfbb | 247 | led1 = 0; |
keeswieriks | 29:9010b7bdbfbb | 248 | led2 = 1; |
keeswieriks | 29:9010b7bdbfbb | 249 | led3 = 1; |
keeswieriks | 29:9010b7bdbfbb | 250 | Filter_tick.attach(&filtering,T); //EMG signals filtered every T sec. |
keeswieriks | 29:9010b7bdbfbb | 251 | MovAg_tick.attach(&MovAg,T); //Moving average calculation every T sec. |
keeswieriks | 29:9010b7bdbfbb | 252 | //ticker.attach(&dv_des_calculate_qref,T); //v_des determined every T |
keeswieriks | 29:9010b7bdbfbb | 253 | |
keeswieriks | 29:9010b7bdbfbb | 254 | // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec. |
keeswieriks | 29:9010b7bdbfbb | 255 | |
keeswieriks | 29:9010b7bdbfbb | 256 | but1.rise(switch_to_calibrate); //Switch state of calibration (which muscle) |
keeswieriks | 29:9010b7bdbfbb | 257 | wait(0.2f); //Wait to avoid bouncing of button |
keeswieriks | 29:9010b7bdbfbb | 258 | but2.rise(calibrate); //Calibrate threshold for 3 muscles |
keeswieriks | 29:9010b7bdbfbb | 259 | wait(0.2f); //Wait to avoid bouncing of button |
keeswieriks | 29:9010b7bdbfbb | 260 | |
keeswieriks | 29:9010b7bdbfbb | 261 | pc.printf("g is %i\n\r",g); |
keeswieriks | 29:9010b7bdbfbb | 262 | pc.printf("Average0 = %f , Average1 = %f, Average2 = %f \n\r",Average0, Average1, Average2); |
keeswieriks | 29:9010b7bdbfbb | 263 | pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2); |
keeswieriks | 29:9010b7bdbfbb | 264 | //wait(2.0f); |
keeswieriks | 29:9010b7bdbfbb | 265 | } |
keeswieriks | 29:9010b7bdbfbb | 266 | } |
keeswieriks | 29:9010b7bdbfbb | 267 | |
keeswieriks | 29:9010b7bdbfbb | 268 |