emg2
Dependencies: HIDScope biquadFilter mbed QEI
Fork of EMG by
main.cpp@27:6b4814ef266d, 2018-10-30 (annotated)
- Committer:
- keeswieriks
- Date:
- Tue Oct 30 10:06:24 2018 +0000
- Revision:
- 27:6b4814ef266d
- Parent:
- 26:1eafb6111ae8
- Child:
- 28:433d12c52913
beter;
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" |
vsluiter | 0:32bb76391d89 | 4 | |
s1574396 | 25:02f183b944ed | 5 | HIDScope scope( 6 ); |
s1574396 | 22:bcfee9594007 | 6 | Ticker sample_timer; |
s1574396 | 22:bcfee9594007 | 7 | |
s1574396 | 23:dec549767006 | 8 | // Inputs EMG |
keeswieriks | 21:931fe86dbf5a | 9 | AnalogIn emg0_in( A0 ); |
keeswieriks | 21:931fe86dbf5a | 10 | AnalogIn emg1_in( A1 ); |
keeswieriks | 21:931fe86dbf5a | 11 | AnalogIn emg2_in( A2 ); |
keeswieriks | 21:931fe86dbf5a | 12 | |
s1574396 | 23:dec549767006 | 13 | // Constants EMG |
s1574396 | 23:dec549767006 | 14 | const double m1 = 0.5000; |
s1574396 | 23:dec549767006 | 15 | const double m2 = -0.8090; |
s1574396 | 23:dec549767006 | 16 | const double n0 = 0.5000; |
s1574396 | 23:dec549767006 | 17 | const double n1 = -0.8090; |
s1574396 | 23:dec549767006 | 18 | const double n2 = 0; |
s1574396 | 23:dec549767006 | 19 | const double a1 = 0.9565; |
s1574396 | 23:dec549767006 | 20 | const double a2 = -1.9131; |
s1574396 | 23:dec549767006 | 21 | const double b0 = 0.9565; |
s1574396 | 23:dec549767006 | 22 | const double b1 = -1.9112; |
s1574396 | 23:dec549767006 | 23 | const double b2 = 0.9150; |
s1574396 | 23:dec549767006 | 24 | const double c1 = 0.0675; |
s1574396 | 23:dec549767006 | 25 | const double c2 = 0.1349; |
s1574396 | 23:dec549767006 | 26 | const double d0 = 0.0675; |
s1574396 | 23:dec549767006 | 27 | const double d1 = -1.1430; |
s1574396 | 23:dec549767006 | 28 | const double d2 = 0.4128; |
s1574396 | 24:6bdc50e21805 | 29 | |
s1574396 | 23:dec549767006 | 30 | // Variables EMG |
s1574396 | 24:6bdc50e21805 | 31 | double emg0; |
s1574396 | 24:6bdc50e21805 | 32 | double emg1; |
s1574396 | 24:6bdc50e21805 | 33 | double emg2; |
s1574396 | 24:6bdc50e21805 | 34 | double notch0; |
s1574396 | 24:6bdc50e21805 | 35 | double notch1; |
s1574396 | 24:6bdc50e21805 | 36 | double notch2; |
s1574396 | 24:6bdc50e21805 | 37 | double high0; |
s1574396 | 24:6bdc50e21805 | 38 | double high1; |
s1574396 | 24:6bdc50e21805 | 39 | double high2; |
s1574396 | 24:6bdc50e21805 | 40 | double absolute0; |
s1574396 | 24:6bdc50e21805 | 41 | double absolute1; |
s1574396 | 24:6bdc50e21805 | 42 | double absolute2; |
s1574396 | 24:6bdc50e21805 | 43 | double low0; |
s1574396 | 24:6bdc50e21805 | 44 | double low1; |
s1574396 | 24:6bdc50e21805 | 45 | double low2; |
keeswieriks | 21:931fe86dbf5a | 46 | |
keeswieriks | 21:931fe86dbf5a | 47 | // BiQuad values |
keeswieriks | 21:931fe86dbf5a | 48 | BiQuadChain notch; |
keeswieriks | 21:931fe86dbf5a | 49 | BiQuad N1( m1, m2, n0, n1, n2); |
keeswieriks | 21:931fe86dbf5a | 50 | BiQuad N2( m1, m2, n0, n1, n2); |
keeswieriks | 21:931fe86dbf5a | 51 | BiQuad N3( m1, m2, n0, n1, n2); |
keeswieriks | 21:931fe86dbf5a | 52 | BiQuadChain highpass; |
keeswieriks | 21:931fe86dbf5a | 53 | BiQuad H1( a1, a2, b0, b1, b2); |
keeswieriks | 21:931fe86dbf5a | 54 | BiQuad H2( a1, a2, b0, b1, b2); |
keeswieriks | 21:931fe86dbf5a | 55 | BiQuad H3( a1, a2, b0, b1, b2); |
keeswieriks | 21:931fe86dbf5a | 56 | BiQuadChain lowpass; |
keeswieriks | 21:931fe86dbf5a | 57 | BiQuad L1( c1, c2, d0, d1, d2); |
keeswieriks | 21:931fe86dbf5a | 58 | BiQuad L2( c1, c2, d0, d1, d2); |
keeswieriks | 21:931fe86dbf5a | 59 | BiQuad L3( c1, c2, d0, d1, d2); |
keeswieriks | 21:931fe86dbf5a | 60 | |
keeswieriks | 26:1eafb6111ae8 | 61 | // EMG |
keeswieriks | 27:6b4814ef266d | 62 | const int sizeMovAg = 100; //Size of array over which the moving average (MovAg) is calculated |
keeswieriks | 26:1eafb6111ae8 | 63 | double sum, sum1, sum2, sum3; //Variables used in calibration and MovAg to sum the elements in the array |
keeswieriks | 26:1eafb6111ae8 | 64 | double StoreArray0[sizeMovAg] = {}, StoreArray1[sizeMovAg] = {}, StoreArray2[sizeMovAg] = {}; |
keeswieriks | 26:1eafb6111ae8 | 65 | |
keeswieriks | 26:1eafb6111ae8 | 66 | //Empty arrays to calculate MovAgs |
keeswieriks | 26:1eafb6111ae8 | 67 | double Average0, Average1, Average2; //Outcome of MovAg |
keeswieriks | 26:1eafb6111ae8 | 68 | const int sizeCali = 2000; //Size of array over which the Threshold will be calculated |
keeswieriks | 26:1eafb6111ae8 | 69 | double StoreCali0[sizeCali] = {}, StoreCali1[sizeCali] = {}, StoreCali2[sizeCali] = {}; |
keeswieriks | 26:1eafb6111ae8 | 70 | //Empty arrays to calculate means in calibration |
keeswieriks | 26:1eafb6111ae8 | 71 | |
keeswieriks | 26:1eafb6111ae8 | 72 | double Mean0, Mean1, Mean2; //Mean of maximum contraction, calculated in the calibration |
keeswieriks | 26:1eafb6111ae8 | 73 | double Threshold0 = 1, Threshold1 = 1, Threshold2 = 1; //Thresholds for muscles 0 to 2 |
keeswieriks | 26:1eafb6111ae8 | 74 | int g = 0; //Part of the switch void, where the current state can be changed |
keeswieriks | 26:1eafb6111ae8 | 75 | int emg_calib=0; //After calibration this value will be 1, enabling the |
keeswieriks | 26:1eafb6111ae8 | 76 | |
keeswieriks | 26:1eafb6111ae8 | 77 | //EMG |
keeswieriks | 26:1eafb6111ae8 | 78 | Ticker Filter_tick; |
keeswieriks | 26:1eafb6111ae8 | 79 | Ticker MovAg_tick; |
keeswieriks | 26:1eafb6111ae8 | 80 | |
s1574396 | 22:bcfee9594007 | 81 | // Filter of the first EMG signal |
s1574396 | 24:6bdc50e21805 | 82 | void filtering() |
vsluiter | 2:e314bb3b2d99 | 83 | { |
s1574396 | 23:dec549767006 | 84 | emg0 = emg0_in.read(); // Reading the EMG signal |
s1574396 | 24:6bdc50e21805 | 85 | emg1 = emg1_in.read(); |
s1574396 | 24:6bdc50e21805 | 86 | emg2 = emg2_in.read(); |
s1574396 | 24:6bdc50e21805 | 87 | notch0 = N1.step(emg0); // Applying a notch filter over the EMG data |
s1574396 | 24:6bdc50e21805 | 88 | notch1 = N2.step(emg1); |
s1574396 | 24:6bdc50e21805 | 89 | notch2 = N3.step(emg2); |
s1574396 | 24:6bdc50e21805 | 90 | high0 = H1.step(notch0); // Applying a high pass filter |
s1574396 | 24:6bdc50e21805 | 91 | high1 = H2.step(notch1); |
s1574396 | 24:6bdc50e21805 | 92 | high2 = H3.step(notch2); |
s1574396 | 24:6bdc50e21805 | 93 | absolute0 = fabs(high0); // Rectifying the signal |
s1574396 | 24:6bdc50e21805 | 94 | absolute1 = fabs(high1); |
s1574396 | 24:6bdc50e21805 | 95 | absolute2 = fabs(high2); |
s1574396 | 24:6bdc50e21805 | 96 | low0 = L1.step(absolute0); // Applying low pass filter |
s1574396 | 24:6bdc50e21805 | 97 | low1 = L2.step(absolute1); |
s1574396 | 24:6bdc50e21805 | 98 | low2 = L3.step(absolute2); |
s1574396 | 23:dec549767006 | 99 | |
keeswieriks | 26:1eafb6111ae8 | 100 | for (int i = sizeMovAg-1; i>=0; i--) { |
keeswieriks | 26:1eafb6111ae8 | 101 | //For statement to make an array of the last datapoints of the filtered signal |
keeswieriks | 26:1eafb6111ae8 | 102 | StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right |
keeswieriks | 26:1eafb6111ae8 | 103 | StoreArray1[i] = StoreArray1[i-1]; |
keeswieriks | 26:1eafb6111ae8 | 104 | StoreArray2[i] = StoreArray2[i-1]; |
keeswieriks | 26:1eafb6111ae8 | 105 | } |
keeswieriks | 26:1eafb6111ae8 | 106 | StoreArray0[0] = low0; //Stores the latest datapoint in the first element of the array |
keeswieriks | 26:1eafb6111ae8 | 107 | StoreArray1[0] = low1; |
keeswieriks | 26:1eafb6111ae8 | 108 | StoreArray2[0] = low2; |
keeswieriks | 26:1eafb6111ae8 | 109 | sum1 = 0.0; |
keeswieriks | 26:1eafb6111ae8 | 110 | sum2 = 0.0; |
keeswieriks | 26:1eafb6111ae8 | 111 | sum3 = 0.0; |
keeswieriks | 26:1eafb6111ae8 | 112 | for (int a = 0; a<=sizeMovAg-1; a++) { //For statement to sum the elements in the array |
keeswieriks | 26:1eafb6111ae8 | 113 | sum1+=StoreArray0[a]; |
keeswieriks | 26:1eafb6111ae8 | 114 | sum2+=StoreArray1[a]; |
keeswieriks | 26:1eafb6111ae8 | 115 | sum3+=StoreArray2[a]; |
keeswieriks | 26:1eafb6111ae8 | 116 | } |
keeswieriks | 26:1eafb6111ae8 | 117 | Average0 = sum1/sizeMovAg; //Calculates an average over the datapoints in the array |
keeswieriks | 26:1eafb6111ae8 | 118 | Average1 = sum2/sizeMovAg; |
keeswieriks | 26:1eafb6111ae8 | 119 | Average2 = sum3/sizeMovAg; |
keeswieriks | 26:1eafb6111ae8 | 120 | |
s1574396 | 25:02f183b944ed | 121 | scope.set( 0, emg0); // Sending the signal to the HIDScope |
s1574396 | 25:02f183b944ed | 122 | scope.set( 1, low0); // Change the numer of inputs on the top when necessary |
keeswieriks | 27:6b4814ef266d | 123 | scope.set( 2, Average0); |
s1574396 | 25:02f183b944ed | 124 | scope.set( 3, low1); |
s1574396 | 25:02f183b944ed | 125 | scope.set( 4, emg2); |
s1574396 | 25:02f183b944ed | 126 | scope.set( 5, low2); |
s1574396 | 24:6bdc50e21805 | 127 | scope.send(); |
keeswieriks | 21:931fe86dbf5a | 128 | } |
keeswieriks | 21:931fe86dbf5a | 129 | |
keeswieriks | 21:931fe86dbf5a | 130 | int main() |
keeswieriks | 21:931fe86dbf5a | 131 | { |
s1574396 | 25:02f183b944ed | 132 | sample_timer.attach( &filtering, 0.002); |
keeswieriks | 21:931fe86dbf5a | 133 | |
s1574396 | 23:dec549767006 | 134 | while(1) {} |
keeswieriks | 26:1eafb6111ae8 | 135 | } |
keeswieriks | 26:1eafb6111ae8 | 136 | |
keeswieriks | 26:1eafb6111ae8 | 137 | |
keeswieriks | 26:1eafb6111ae8 | 138 | |
keeswieriks | 26:1eafb6111ae8 | 139 | void MovAg() //Void to make a moving average |
keeswieriks | 26:1eafb6111ae8 | 140 | { |
keeswieriks | 26:1eafb6111ae8 | 141 | |
vsluiter | 0:32bb76391d89 | 142 | } |