emg2
Dependencies: HIDScope biquadFilter mbed QEI
Fork of EMG by
main.cpp@28:433d12c52913, 2018-10-30 (annotated)
- Committer:
- keeswieriks
- Date:
- Tue Oct 30 10:23:46 2018 +0000
- Revision:
- 28:433d12c52913
- Parent:
- 27:6b4814ef266d
- Child:
- 29:9010b7bdbfbb
met emg cali er bijna 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" |
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 | |
keeswieriks | 28:433d12c52913 | 13 | DigitalOut led1(LED_BLUE); |
keeswieriks | 28:433d12c52913 | 14 | DigitalOut led2(LED_RED); |
keeswieriks | 28:433d12c52913 | 15 | DigitalOut led3(LED_GREEN); |
keeswieriks | 28:433d12c52913 | 16 | |
s1574396 | 23:dec549767006 | 17 | // Constants EMG |
s1574396 | 23:dec549767006 | 18 | const double m1 = 0.5000; |
s1574396 | 23:dec549767006 | 19 | const double m2 = -0.8090; |
s1574396 | 23:dec549767006 | 20 | const double n0 = 0.5000; |
s1574396 | 23:dec549767006 | 21 | const double n1 = -0.8090; |
s1574396 | 23:dec549767006 | 22 | const double n2 = 0; |
s1574396 | 23:dec549767006 | 23 | const double a1 = 0.9565; |
s1574396 | 23:dec549767006 | 24 | const double a2 = -1.9131; |
s1574396 | 23:dec549767006 | 25 | const double b0 = 0.9565; |
s1574396 | 23:dec549767006 | 26 | const double b1 = -1.9112; |
s1574396 | 23:dec549767006 | 27 | const double b2 = 0.9150; |
s1574396 | 23:dec549767006 | 28 | const double c1 = 0.0675; |
s1574396 | 23:dec549767006 | 29 | const double c2 = 0.1349; |
s1574396 | 23:dec549767006 | 30 | const double d0 = 0.0675; |
s1574396 | 23:dec549767006 | 31 | const double d1 = -1.1430; |
s1574396 | 23:dec549767006 | 32 | const double d2 = 0.4128; |
s1574396 | 24:6bdc50e21805 | 33 | |
s1574396 | 23:dec549767006 | 34 | // Variables EMG |
s1574396 | 24:6bdc50e21805 | 35 | double emg0; |
s1574396 | 24:6bdc50e21805 | 36 | double emg1; |
s1574396 | 24:6bdc50e21805 | 37 | double emg2; |
s1574396 | 24:6bdc50e21805 | 38 | double notch0; |
s1574396 | 24:6bdc50e21805 | 39 | double notch1; |
s1574396 | 24:6bdc50e21805 | 40 | double notch2; |
s1574396 | 24:6bdc50e21805 | 41 | double high0; |
s1574396 | 24:6bdc50e21805 | 42 | double high1; |
s1574396 | 24:6bdc50e21805 | 43 | double high2; |
s1574396 | 24:6bdc50e21805 | 44 | double absolute0; |
s1574396 | 24:6bdc50e21805 | 45 | double absolute1; |
s1574396 | 24:6bdc50e21805 | 46 | double absolute2; |
s1574396 | 24:6bdc50e21805 | 47 | double low0; |
s1574396 | 24:6bdc50e21805 | 48 | double low1; |
s1574396 | 24:6bdc50e21805 | 49 | double low2; |
keeswieriks | 21:931fe86dbf5a | 50 | |
keeswieriks | 21:931fe86dbf5a | 51 | // BiQuad values |
keeswieriks | 21:931fe86dbf5a | 52 | BiQuadChain notch; |
keeswieriks | 21:931fe86dbf5a | 53 | BiQuad N1( m1, m2, n0, n1, n2); |
keeswieriks | 21:931fe86dbf5a | 54 | BiQuad N2( m1, m2, n0, n1, n2); |
keeswieriks | 21:931fe86dbf5a | 55 | BiQuad N3( m1, m2, n0, n1, n2); |
keeswieriks | 21:931fe86dbf5a | 56 | BiQuadChain highpass; |
keeswieriks | 21:931fe86dbf5a | 57 | BiQuad H1( a1, a2, b0, b1, b2); |
keeswieriks | 21:931fe86dbf5a | 58 | BiQuad H2( a1, a2, b0, b1, b2); |
keeswieriks | 21:931fe86dbf5a | 59 | BiQuad H3( a1, a2, b0, b1, b2); |
keeswieriks | 21:931fe86dbf5a | 60 | BiQuadChain lowpass; |
keeswieriks | 21:931fe86dbf5a | 61 | BiQuad L1( c1, c2, d0, d1, d2); |
keeswieriks | 21:931fe86dbf5a | 62 | BiQuad L2( c1, c2, d0, d1, d2); |
keeswieriks | 21:931fe86dbf5a | 63 | BiQuad L3( c1, c2, d0, d1, d2); |
keeswieriks | 21:931fe86dbf5a | 64 | |
keeswieriks | 26:1eafb6111ae8 | 65 | // EMG |
keeswieriks | 27:6b4814ef266d | 66 | const int sizeMovAg = 100; //Size of array over which the moving average (MovAg) is calculated |
keeswieriks | 26:1eafb6111ae8 | 67 | double sum, sum1, sum2, sum3; //Variables used in calibration and MovAg to sum the elements in the array |
keeswieriks | 26:1eafb6111ae8 | 68 | double StoreArray0[sizeMovAg] = {}, StoreArray1[sizeMovAg] = {}, StoreArray2[sizeMovAg] = {}; |
keeswieriks | 26:1eafb6111ae8 | 69 | |
keeswieriks | 26:1eafb6111ae8 | 70 | //Empty arrays to calculate MovAgs |
keeswieriks | 26:1eafb6111ae8 | 71 | double Average0, Average1, Average2; //Outcome of MovAg |
keeswieriks | 26:1eafb6111ae8 | 72 | const int sizeCali = 2000; //Size of array over which the Threshold will be calculated |
keeswieriks | 26:1eafb6111ae8 | 73 | double StoreCali0[sizeCali] = {}, StoreCali1[sizeCali] = {}, StoreCali2[sizeCali] = {}; |
keeswieriks | 26:1eafb6111ae8 | 74 | //Empty arrays to calculate means in calibration |
keeswieriks | 26:1eafb6111ae8 | 75 | |
keeswieriks | 26:1eafb6111ae8 | 76 | double Mean0, Mean1, Mean2; //Mean of maximum contraction, calculated in the calibration |
keeswieriks | 26:1eafb6111ae8 | 77 | double Threshold0 = 1, Threshold1 = 1, Threshold2 = 1; //Thresholds for muscles 0 to 2 |
keeswieriks | 26:1eafb6111ae8 | 78 | int g = 0; //Part of the switch void, where the current state can be changed |
keeswieriks | 26:1eafb6111ae8 | 79 | int emg_calib=0; //After calibration this value will be 1, enabling the |
keeswieriks | 26:1eafb6111ae8 | 80 | |
keeswieriks | 26:1eafb6111ae8 | 81 | //EMG |
keeswieriks | 26:1eafb6111ae8 | 82 | Ticker Filter_tick; |
keeswieriks | 26:1eafb6111ae8 | 83 | Ticker MovAg_tick; |
keeswieriks | 26:1eafb6111ae8 | 84 | |
s1574396 | 22:bcfee9594007 | 85 | // Filter of the first EMG signal |
s1574396 | 24:6bdc50e21805 | 86 | void filtering() |
vsluiter | 2:e314bb3b2d99 | 87 | { |
s1574396 | 23:dec549767006 | 88 | emg0 = emg0_in.read(); // Reading the EMG signal |
s1574396 | 24:6bdc50e21805 | 89 | emg1 = emg1_in.read(); |
s1574396 | 24:6bdc50e21805 | 90 | emg2 = emg2_in.read(); |
s1574396 | 24:6bdc50e21805 | 91 | notch0 = N1.step(emg0); // Applying a notch filter over the EMG data |
s1574396 | 24:6bdc50e21805 | 92 | notch1 = N2.step(emg1); |
s1574396 | 24:6bdc50e21805 | 93 | notch2 = N3.step(emg2); |
s1574396 | 24:6bdc50e21805 | 94 | high0 = H1.step(notch0); // Applying a high pass filter |
s1574396 | 24:6bdc50e21805 | 95 | high1 = H2.step(notch1); |
s1574396 | 24:6bdc50e21805 | 96 | high2 = H3.step(notch2); |
s1574396 | 24:6bdc50e21805 | 97 | absolute0 = fabs(high0); // Rectifying the signal |
s1574396 | 24:6bdc50e21805 | 98 | absolute1 = fabs(high1); |
s1574396 | 24:6bdc50e21805 | 99 | absolute2 = fabs(high2); |
s1574396 | 24:6bdc50e21805 | 100 | low0 = L1.step(absolute0); // Applying low pass filter |
s1574396 | 24:6bdc50e21805 | 101 | low1 = L2.step(absolute1); |
s1574396 | 24:6bdc50e21805 | 102 | low2 = L3.step(absolute2); |
s1574396 | 23:dec549767006 | 103 | |
keeswieriks | 26:1eafb6111ae8 | 104 | for (int i = sizeMovAg-1; i>=0; i--) { |
keeswieriks | 26:1eafb6111ae8 | 105 | //For statement to make an array of the last datapoints of the filtered signal |
keeswieriks | 26:1eafb6111ae8 | 106 | StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right |
keeswieriks | 26:1eafb6111ae8 | 107 | StoreArray1[i] = StoreArray1[i-1]; |
keeswieriks | 26:1eafb6111ae8 | 108 | StoreArray2[i] = StoreArray2[i-1]; |
keeswieriks | 26:1eafb6111ae8 | 109 | } |
keeswieriks | 26:1eafb6111ae8 | 110 | StoreArray0[0] = low0; //Stores the latest datapoint in the first element of the array |
keeswieriks | 26:1eafb6111ae8 | 111 | StoreArray1[0] = low1; |
keeswieriks | 26:1eafb6111ae8 | 112 | StoreArray2[0] = low2; |
keeswieriks | 26:1eafb6111ae8 | 113 | sum1 = 0.0; |
keeswieriks | 26:1eafb6111ae8 | 114 | sum2 = 0.0; |
keeswieriks | 26:1eafb6111ae8 | 115 | sum3 = 0.0; |
keeswieriks | 26:1eafb6111ae8 | 116 | for (int a = 0; a<=sizeMovAg-1; a++) { //For statement to sum the elements in the array |
keeswieriks | 26:1eafb6111ae8 | 117 | sum1+=StoreArray0[a]; |
keeswieriks | 26:1eafb6111ae8 | 118 | sum2+=StoreArray1[a]; |
keeswieriks | 26:1eafb6111ae8 | 119 | sum3+=StoreArray2[a]; |
keeswieriks | 26:1eafb6111ae8 | 120 | } |
keeswieriks | 26:1eafb6111ae8 | 121 | Average0 = sum1/sizeMovAg; //Calculates an average over the datapoints in the array |
keeswieriks | 26:1eafb6111ae8 | 122 | Average1 = sum2/sizeMovAg; |
keeswieriks | 26:1eafb6111ae8 | 123 | Average2 = sum3/sizeMovAg; |
keeswieriks | 26:1eafb6111ae8 | 124 | |
s1574396 | 25:02f183b944ed | 125 | scope.set( 0, emg0); // Sending the signal to the HIDScope |
s1574396 | 25:02f183b944ed | 126 | scope.set( 1, low0); // Change the numer of inputs on the top when necessary |
keeswieriks | 27:6b4814ef266d | 127 | scope.set( 2, Average0); |
s1574396 | 25:02f183b944ed | 128 | scope.set( 3, low1); |
s1574396 | 25:02f183b944ed | 129 | scope.set( 4, emg2); |
s1574396 | 25:02f183b944ed | 130 | scope.set( 5, low2); |
s1574396 | 24:6bdc50e21805 | 131 | scope.send(); |
keeswieriks | 21:931fe86dbf5a | 132 | } |
keeswieriks | 21:931fe86dbf5a | 133 | |
keeswieriks | 21:931fe86dbf5a | 134 | int main() |
keeswieriks | 21:931fe86dbf5a | 135 | { |
s1574396 | 25:02f183b944ed | 136 | sample_timer.attach( &filtering, 0.002); |
keeswieriks | 21:931fe86dbf5a | 137 | |
s1574396 | 23:dec549767006 | 138 | while(1) {} |
keeswieriks | 26:1eafb6111ae8 | 139 | } |
keeswieriks | 26:1eafb6111ae8 | 140 | |
keeswieriks | 26:1eafb6111ae8 | 141 | |
keeswieriks | 26:1eafb6111ae8 | 142 | |
keeswieriks | 28:433d12c52913 | 143 | void Switching() //Void to switch between signals to calibrate |
keeswieriks | 28:433d12c52913 | 144 | { |
keeswieriks | 28:433d12c52913 | 145 | g++; |
keeswieriks | 28:433d12c52913 | 146 | if (g == 0) |
keeswieriks | 28:433d12c52913 | 147 | { //If g = 0, led is blue |
keeswieriks | 28:433d12c52913 | 148 | led1=0; |
keeswieriks | 28:433d12c52913 | 149 | led2=1; |
keeswieriks | 28:433d12c52913 | 150 | led3=1; |
keeswieriks | 28:433d12c52913 | 151 | } |
keeswieriks | 28:433d12c52913 | 152 | else if(g == 1) |
keeswieriks | 28:433d12c52913 | 153 | { //If g = 1, led is red |
keeswieriks | 28:433d12c52913 | 154 | led1=1; |
keeswieriks | 28:433d12c52913 | 155 | led2=0; |
keeswieriks | 28:433d12c52913 | 156 | led3=1; |
keeswieriks | 28:433d12c52913 | 157 | |
keeswieriks | 28:433d12c52913 | 158 | } |
keeswieriks | 28:433d12c52913 | 159 | else if(g == 2) |
keeswieriks | 28:433d12c52913 | 160 | { //If g = 2, led is green |
keeswieriks | 28:433d12c52913 | 161 | led1=1; |
keeswieriks | 28:433d12c52913 | 162 | led2=1; |
keeswieriks | 28:433d12c52913 | 163 | led3=0; |
keeswieriks | 28:433d12c52913 | 164 | } |
keeswieriks | 28:433d12c52913 | 165 | else |
keeswieriks | 28:433d12c52913 | 166 | { //If g > 3, led is white |
keeswieriks | 28:433d12c52913 | 167 | led1=0; |
keeswieriks | 28:433d12c52913 | 168 | led2=0; |
keeswieriks | 28:433d12c52913 | 169 | led3=0; |
keeswieriks | 28:433d12c52913 | 170 | } |
keeswieriks | 28:433d12c52913 | 171 | } |
keeswieriks | 28:433d12c52913 | 172 | void Calibration(void) //Void to calibrate the signals, depends on value g. While calibrating, maximal contraction is required |
keeswieriks | 28:433d12c52913 | 173 | |
keeswieriks | 26:1eafb6111ae8 | 174 | { |
keeswieriks | 28:433d12c52913 | 175 | switch (g) { |
keeswieriks | 28:433d12c52913 | 176 | case 0: { //Case zero, calibrate EMG signal of right biceps |
keeswieriks | 28:433d12c52913 | 177 | sum = 0.0; |
keeswieriks | 28:433d12c52913 | 178 | for (int j = 0; j<=sizeCali-1; j++) { |
keeswieriks | 28:433d12c52913 | 179 | //For statement to make an array of the latest datapoints of the filtered signal |
keeswieriks | 28:433d12c52913 | 180 | StoreCali0[j] = low0; //Stores the latest datapoint in the first element of the array |
keeswieriks | 28:433d12c52913 | 181 | sum+=StoreCali0[j]; //Sums the elements in the array |
keeswieriks | 28:433d12c52913 | 182 | wait(0.001f); |
keeswieriks | 28:433d12c52913 | 183 | } |
keeswieriks | 28:433d12c52913 | 184 | Mean0 = sum/sizeCali; //Calculates the mean of the signal |
keeswieriks | 28:433d12c52913 | 185 | Threshold0 = Mean0/2; //Determines the threshold (factor 0.5) |
keeswieriks | 28:433d12c52913 | 186 | break; |
keeswieriks | 28:433d12c52913 | 187 | } |
keeswieriks | 28:433d12c52913 | 188 | case 1: { //Case one, calibrate EMG signal of left biceps |
keeswieriks | 28:433d12c52913 | 189 | sum = 0.0; |
keeswieriks | 28:433d12c52913 | 190 | for(int j=0; j<=sizeCali-1; j++) { |
keeswieriks | 28:433d12c52913 | 191 | StoreCali1[j] = low1; |
keeswieriks | 28:433d12c52913 | 192 | sum+=StoreCali1[j]; |
keeswieriks | 28:433d12c52913 | 193 | wait(0.001f); |
keeswieriks | 28:433d12c52913 | 194 | } |
keeswieriks | 28:433d12c52913 | 195 | Mean1 = sum/sizeCali; |
keeswieriks | 28:433d12c52913 | 196 | Threshold1 = Mean1/2; |
keeswieriks | 28:433d12c52913 | 197 | break; |
keeswieriks | 28:433d12c52913 | 198 | } |
keeswieriks | 28:433d12c52913 | 199 | case 2: { //case two, calibrate EMG signal of calf |
keeswieriks | 28:433d12c52913 | 200 | sum = 0.0; |
keeswieriks | 28:433d12c52913 | 201 | for(int j=0; j<=sizeCali-1; j++) { |
keeswieriks | 28:433d12c52913 | 202 | StoreCali2[j] = low2; |
keeswieriks | 28:433d12c52913 | 203 | sum+=StoreCali2[j]; |
keeswieriks | 28:433d12c52913 | 204 | wait(0.001f); |
keeswieriks | 28:433d12c52913 | 205 | } |
keeswieriks | 28:433d12c52913 | 206 | Mean2 = sum/sizeCali; |
keeswieriks | 28:433d12c52913 | 207 | Threshold2 = Mean2/2; |
keeswieriks | 28:433d12c52913 | 208 | break; |
keeswieriks | 28:433d12c52913 | 209 | } |
keeswieriks | 28:433d12c52913 | 210 | case 3: { //Sets calibration value to 1; robot can be set to Home position |
keeswieriks | 28:433d12c52913 | 211 | emg_calib=1; |
keeswieriks | 28:433d12c52913 | 212 | wait(0.001f); |
keeswieriks | 28:433d12c52913 | 213 | break; |
keeswieriks | 28:433d12c52913 | 214 | } |
keeswieriks | 28:433d12c52913 | 215 | default: { //Ensures nothing happens if g is not equal to 0,1 or 2. |
keeswieriks | 28:433d12c52913 | 216 | break; |
keeswieriks | 28:433d12c52913 | 217 | } |
keeswieriks | 28:433d12c52913 | 218 | } |
vsluiter | 0:32bb76391d89 | 219 | } |