Het totale EMG script
Dependencies: HIDScope MODSERIAL biquadFilter mbed
Fork of EMGV5 by
main.cpp@31:a17c20e0614e, 2015-10-15 (annotated)
- Committer:
- Margreeth95
- Date:
- Thu Oct 15 13:08:19 2015 +0000
- Revision:
- 31:a17c20e0614e
- Parent:
- 30:29c9e9d0e398
Voor groep
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" |
Margreeth95 | 19:34456a1effc4 | 3 | #include "MODSERIAL.h" |
Margreeth95 | 19:34456a1effc4 | 4 | #include "biquadFilter.h" |
vsluiter | 0:32bb76391d89 | 5 | |
Margreeth95 | 31:a17c20e0614e | 6 | // Test voor remote repository |
vsluiter | 4:8b298dfada81 | 7 | //Define objects |
Margreeth95 | 18:12250e88037f | 8 | AnalogIn EMG_left(A0); //Analog input |
Margreeth95 | 18:12250e88037f | 9 | AnalogIn EMG_right(A1); |
Margreeth95 | 27:aa5ffd7227a5 | 10 | Ticker SampleEMGLeft; |
Margreeth95 | 27:aa5ffd7227a5 | 11 | Ticker SampleEMGRight; |
Margreeth95 | 18:12250e88037f | 12 | Ticker ScopeTimer; |
Margreeth95 | 19:34456a1effc4 | 13 | Ticker serial; |
Margreeth95 | 27:aa5ffd7227a5 | 14 | Ticker MovingAverageLeft; |
Margreeth95 | 27:aa5ffd7227a5 | 15 | Ticker MovingAverageRight; |
Margreeth95 | 30:29c9e9d0e398 | 16 | HIDScope scope(3); |
Margreeth95 | 19:34456a1effc4 | 17 | DigitalOut led(LED_RED); |
laura94 | 29:c43e9f78db37 | 18 | DigitalOut ledG(LED_GREEN); |
Margreeth95 | 30:29c9e9d0e398 | 19 | DigitalOut ledB(LED_BLUE); |
Margreeth95 | 19:34456a1effc4 | 20 | MODSERIAL pc(USBTX, USBRX); |
Margreeth95 | 19:34456a1effc4 | 21 | |
Margreeth95 | 18:12250e88037f | 22 | |
Margreeth95 | 19:34456a1effc4 | 23 | // Declaring variables |
Margreeth95 | 18:12250e88037f | 24 | double EMG_L_f_v1 = 0, EMG_L_f_v2 = 0; |
Margreeth95 | 19:34456a1effc4 | 25 | double EMG_L_fh=0; |
Margreeth95 | 19:34456a1effc4 | 26 | double EMG_left_value; |
Margreeth95 | 22:207d314523ef | 27 | double EMG_left_f1; |
Margreeth95 | 22:207d314523ef | 28 | double EMG_left_f2; |
Margreeth95 | 24:6661dd6a1718 | 29 | double EMG_left_f3; |
laura94 | 29:c43e9f78db37 | 30 | double EMG_left_abs; |
Margreeth95 | 27:aa5ffd7227a5 | 31 | |
Margreeth95 | 22:207d314523ef | 32 | double EMG_right_value; |
Margreeth95 | 22:207d314523ef | 33 | double EMG_right_f1; |
Margreeth95 | 22:207d314523ef | 34 | double EMG_right_f2; |
Margreeth95 | 25:5e9cc994cd1f | 35 | double EMG_right_f3; |
Margreeth95 | 22:207d314523ef | 36 | double EMG_right_abs; |
laura94 | 29:c43e9f78db37 | 37 | double Threshold1; |
Margreeth95 | 30:29c9e9d0e398 | 38 | double Threshold2; |
Margreeth95 | 30:29c9e9d0e398 | 39 | double Threshold3; |
Margreeth95 | 30:29c9e9d0e398 | 40 | double Threshold4; |
Margreeth95 | 24:6661dd6a1718 | 41 | |
Margreeth95 | 25:5e9cc994cd1f | 42 | int N = 50; |
Margreeth95 | 25:5e9cc994cd1f | 43 | double MAF_left[50]; |
Margreeth95 | 24:6661dd6a1718 | 44 | double EMG_left_MAF; |
Margreeth95 | 27:aa5ffd7227a5 | 45 | double MAF_right[50]; |
Margreeth95 | 27:aa5ffd7227a5 | 46 | double EMG_right_MAF; |
Margreeth95 | 24:6661dd6a1718 | 47 | |
Margreeth95 | 18:12250e88037f | 48 | // coëfficiënten |
laura94 | 29:c43e9f78db37 | 49 | const double BiGainEMG_H1 = 0.796821; |
laura94 | 29:c43e9f78db37 | 50 | const double EMGH1_a1 = -1.47500228332, EMGH1_a2 = 0.55273994299, EMGH1_b0 = 1.0*BiGainEMG_H1, EMGH1_b1 = -1.99922446977*BiGainEMG_H1, EMGH1_b2 = 1.0*BiGainEMG_H1; //coefficients for high-pass filter |
Margreeth95 | 24:6661dd6a1718 | 51 | |
laura94 | 29:c43e9f78db37 | 52 | const double BiGainEMG_L1= 0.001041; |
laura94 | 29:c43e9f78db37 | 53 | const double EMGL1_a1 = -1.87506717001, EMGL1_a2 = 0.87923101463, EMGL1_b0 = 1.0*BiGainEMG_L1, EMGL1_b1 = 2.00000000000*BiGainEMG_L1, EMGL1_b2 = 1.0*BiGainEMG_L1; // coefficients for low-pass filter |
laura94 | 29:c43e9f78db37 | 54 | |
laura94 | 29:c43e9f78db37 | 55 | const double BiGainEMG_N1 = 1.0; |
laura94 | 29:c43e9f78db37 | 56 | const double EMGN1_a1 = -1.58174308681, EMGN1_a2 = 0.96540248979, EMGN1_b0 = 1.0*BiGainEMG_N1, EMGN1_b1 = -1.61816176147*BiGainEMG_N1, EMGN1_b2 = 1.0*BiGainEMG_N1; //coefficients for notch filter |
Margreeth95 | 24:6661dd6a1718 | 57 | |
Margreeth95 | 20:a0642e983da9 | 58 | // Filter creation |
Margreeth95 | 24:6661dd6a1718 | 59 | biquadFilter EMG_highpass1 (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2); // creates the high pass filter |
laura94 | 29:c43e9f78db37 | 60 | biquadFilter EMG_lowpass1 (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2); // creates the low pass filter |
Margreeth95 | 24:6661dd6a1718 | 61 | biquadFilter EMG_notch1 (EMGN1_a1, EMGN1_a2, EMGN1_b0, EMGN1_b1, EMGN1_b2); // creates the notch filter |
laura94 | 29:c43e9f78db37 | 62 | |
Margreeth95 | 27:aa5ffd7227a5 | 63 | biquadFilter EMG_highpass1R (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2); // creates the high pass filter |
laura94 | 29:c43e9f78db37 | 64 | biquadFilter EMG_lowpass1R (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2); // creates the low pass filter |
Margreeth95 | 27:aa5ffd7227a5 | 65 | biquadFilter EMG_notch1R (EMGN1_a1, EMGN1_a2, EMGN1_b0, EMGN1_b1, EMGN1_b2); // creates the notch filter |
laura94 | 29:c43e9f78db37 | 66 | |
Margreeth95 | 27:aa5ffd7227a5 | 67 | |
Margreeth95 | 19:34456a1effc4 | 68 | // EMG filtering function |
Margreeth95 | 27:aa5ffd7227a5 | 69 | void EMGfilterLeft() |
Margreeth95 | 19:34456a1effc4 | 70 | { |
Margreeth95 | 19:34456a1effc4 | 71 | EMG_left_value = EMG_left.read(); |
Margreeth95 | 24:6661dd6a1718 | 72 | EMG_left_f1 = EMG_highpass1.step(EMG_left_value); |
laura94 | 29:c43e9f78db37 | 73 | EMG_left_abs = fabs(EMG_left_f1); |
laura94 | 29:c43e9f78db37 | 74 | EMG_left_f2 = EMG_lowpass1.step(EMG_left_abs); |
laura94 | 29:c43e9f78db37 | 75 | EMG_left_f3 = EMG_notch1.step(EMG_left_f2); |
Margreeth95 | 27:aa5ffd7227a5 | 76 | |
Margreeth95 | 27:aa5ffd7227a5 | 77 | } |
Margreeth95 | 27:aa5ffd7227a5 | 78 | |
Margreeth95 | 27:aa5ffd7227a5 | 79 | void EMGfilterRight() |
Margreeth95 | 27:aa5ffd7227a5 | 80 | { |
Margreeth95 | 25:5e9cc994cd1f | 81 | EMG_right_value = EMG_right.read(); |
Margreeth95 | 27:aa5ffd7227a5 | 82 | EMG_right_f1 = EMG_highpass1R.step(EMG_right_value); |
laura94 | 29:c43e9f78db37 | 83 | EMG_right_abs = fabs(EMG_right_f1); |
laura94 | 29:c43e9f78db37 | 84 | EMG_right_f2 = EMG_lowpass1R.step(EMG_right_abs); |
laura94 | 29:c43e9f78db37 | 85 | EMG_right_f3 = EMG_notch1R.step(EMG_right_f2); |
Margreeth95 | 19:34456a1effc4 | 86 | } |
vsluiter | 2:e314bb3b2d99 | 87 | |
Margreeth95 | 24:6661dd6a1718 | 88 | // Movingaverage Filter |
Margreeth95 | 27:aa5ffd7227a5 | 89 | void MovingAverageFilterLeft() |
Margreeth95 | 24:6661dd6a1718 | 90 | { |
Margreeth95 | 25:5e9cc994cd1f | 91 | EMG_left_MAF = (MAF_left[0]+MAF_left[1]+MAF_left[2]+MAF_left[3]+MAF_left[4]+MAF_left[5]+MAF_left[6]+MAF_left[7]+MAF_left[8]+MAF_left[9]+MAF_left[10]+MAF_left[11]+MAF_left[12]+MAF_left[13]+MAF_left[14]+MAF_left[15]+MAF_left[16]+MAF_left[17]+MAF_left[18]+MAF_left[19]+MAF_left[20]+MAF_left[21]+MAF_left[22]+MAF_left[23]+MAF_left[24]+MAF_left[25]+MAF_left[26]+MAF_left[27]+MAF_left[28]+MAF_left[29]+MAF_left[30]+MAF_left[31]+MAF_left[32]+MAF_left[33]+MAF_left[34]+MAF_left[35]+MAF_left[36]+MAF_left[37]+MAF_left[38]+MAF_left[39]+MAF_left[40]+MAF_left[41]+MAF_left[42]+MAF_left[43]+MAF_left[44]+MAF_left[45]+MAF_left[46]+MAF_left[47]+MAF_left[48]+MAF_left[49])/N; |
Margreeth95 | 28:38d21b78c6aa | 92 | MAF_left[49] = MAF_left[48], MAF_left[48] = MAF_left[47], MAF_left[47] = MAF_left[46], MAF_left[46] = MAF_left[45], MAF_left[45] = MAF_left[44], MAF_left[44] = MAF_left[43], MAF_left[43] = MAF_left[42], MAF_left[42] = MAF_left[41], MAF_left[41] = MAF_left[40], MAF_left[40] = MAF_left[39], MAF_left[39] = MAF_left[38], MAF_left[38] = MAF_left[37], MAF_left[37] = MAF_left[36], MAF_left[36] = MAF_left[35], MAF_left[35] = MAF_left[34], MAF_left[34] = MAF_left[33], MAF_left[33] = MAF_left[32], MAF_left[32] = MAF_left[31], MAF_left[31] = MAF_left[30], MAF_left[30] = MAF_left[29], MAF_left[29] = MAF_left[28], MAF_left[28] = MAF_left[27], MAF_left[27] = MAF_left[26], MAF_left[26] = MAF_left[25]; |
Margreeth95 | 28:38d21b78c6aa | 93 | MAF_left[25] = MAF_left[24], MAF_left[24] = MAF_left[23], MAF_left[23] = MAF_left[22], MAF_left[22] = MAF_left[21], MAF_left[21] = MAF_left[20], MAF_left[20] = MAF_left[19], MAF_left[19] = MAF_left[18], MAF_left[18] = MAF_left[17], MAF_left[17] = MAF_left[16], MAF_left[16] = MAF_left[15], MAF_left[15] = MAF_left[14], MAF_left[14] = MAF_left[13], MAF_left[13] = MAF_left[12], MAF_left[12] = MAF_left[11], MAF_left[11] = MAF_left[10], MAF_left[10] = MAF_left[9], MAF_left[9] = MAF_left[8], MAF_left[8] = MAF_left[7], MAF_left[7] = MAF_left[6], MAF_left[6] = MAF_left[5], MAF_left[5] = MAF_left[4], MAF_left[4] = MAF_left[3], MAF_left[3] = MAF_left[2], MAF_left[2] = MAF_left[1], MAF_left[1] = MAF_left[0]; |
laura94 | 29:c43e9f78db37 | 94 | MAF_left[0] = EMG_left_f3; |
Margreeth95 | 24:6661dd6a1718 | 95 | } |
Margreeth95 | 24:6661dd6a1718 | 96 | |
Margreeth95 | 27:aa5ffd7227a5 | 97 | void MovingAverageFilterRight() |
Margreeth95 | 27:aa5ffd7227a5 | 98 | { |
Margreeth95 | 27:aa5ffd7227a5 | 99 | EMG_right_MAF = (MAF_right[0]+MAF_right[1]+MAF_right[2]+MAF_right[3]+MAF_right[4]+MAF_right[5]+MAF_right[6]+MAF_right[7]+MAF_right[8]+MAF_right[9]+MAF_right[10]+MAF_right[11]+MAF_right[12]+MAF_right[13]+MAF_right[14]+MAF_right[15]+MAF_right[16]+MAF_right[17]+MAF_right[18]+MAF_right[19]+MAF_right[20]+MAF_right[21]+MAF_right[22]+MAF_right[23]+MAF_right[24]+MAF_right[25]+MAF_right[26]+MAF_right[27]+MAF_right[28]+MAF_right[29]+MAF_right[30]+MAF_right[31]+MAF_right[32]+MAF_right[33]+MAF_right[34]+MAF_right[35]+MAF_right[36]+MAF_right[37]+MAF_right[38]+MAF_right[39]+MAF_right[40]+MAF_right[41]+MAF_right[42]+MAF_right[43]+MAF_right[44]+MAF_right[45]+MAF_right[46]+MAF_right[47]+MAF_right[48]+MAF_right[49])/N; |
Margreeth95 | 28:38d21b78c6aa | 100 | MAF_right[49] = MAF_right[48], MAF_right[48] = MAF_right[47], MAF_right[47] = MAF_right[46], MAF_right[46] = MAF_right[45], MAF_right[45] = MAF_right[44], MAF_right[44] = MAF_right[43], MAF_right[43] = MAF_right[42], MAF_right[42] = MAF_right[41], MAF_right[41] = MAF_right[40], MAF_right[40] = MAF_right[39], MAF_right[39] = MAF_right[38], MAF_right[38] = MAF_right[37], MAF_right[37] = MAF_right[36], MAF_right[36] = MAF_right[35], MAF_right[35] = MAF_right[34], MAF_right[34] = MAF_right[33], MAF_right[33] = MAF_right[32], MAF_right[32] = MAF_right[31], MAF_right[31] = MAF_right[30], MAF_right[30] = MAF_right[29], MAF_right[29] = MAF_right[28], MAF_right[28] = MAF_right[27], MAF_right[27] = MAF_right[26], MAF_right[26] = MAF_right[25]; |
Margreeth95 | 28:38d21b78c6aa | 101 | MAF_right[25] = MAF_right[24], MAF_right[24] = MAF_right[23], MAF_right[23] = MAF_right[22], MAF_right[22] = MAF_right[21], MAF_right[21] = MAF_right[20], MAF_right[20] = MAF_right[19], MAF_right[19] = MAF_right[18], MAF_right[18] = MAF_right[17], MAF_right[17] = MAF_right[16], MAF_right[16] = MAF_right[15], MAF_right[15] = MAF_right[14], MAF_right[14] = MAF_right[13], MAF_right[13] = MAF_right[12], MAF_right[12] = MAF_right[11], MAF_right[11] = MAF_right[10], MAF_right[10] = MAF_right[9], MAF_right[9] = MAF_right[8], MAF_right[8] = MAF_right[7], MAF_right[7] = MAF_right[6], MAF_right[6] = MAF_right[5], MAF_right[5] = MAF_right[4], MAF_right[4] = MAF_right[3], MAF_right[3] = MAF_right[2], MAF_right[2] = MAF_right[1], MAF_right[1] = MAF_right[0]; |
laura94 | 29:c43e9f78db37 | 102 | MAF_right[0] = EMG_right_f3; |
Margreeth95 | 27:aa5ffd7227a5 | 103 | } |
Margreeth95 | 27:aa5ffd7227a5 | 104 | |
Margreeth95 | 27:aa5ffd7227a5 | 105 | |
Margreeth95 | 18:12250e88037f | 106 | // HIDScope |
Margreeth95 | 18:12250e88037f | 107 | void ScopeSend() |
Margreeth95 | 18:12250e88037f | 108 | { |
Margreeth95 | 30:29c9e9d0e398 | 109 | scope.set(0, EMG_left_MAF); |
Margreeth95 | 30:29c9e9d0e398 | 110 | scope.set(1, Threshold1); |
Margreeth95 | 30:29c9e9d0e398 | 111 | scope.set(2, Threshold2); |
laura94 | 29:c43e9f78db37 | 112 | |
Margreeth95 | 18:12250e88037f | 113 | scope.send(); |
Margreeth95 | 18:12250e88037f | 114 | } |
Margreeth95 | 18:12250e88037f | 115 | |
vsluiter | 0:32bb76391d89 | 116 | int main() |
vsluiter | 0:32bb76391d89 | 117 | { |
Margreeth95 | 27:aa5ffd7227a5 | 118 | SampleEMGLeft.attach(&EMGfilterLeft, 0.002); |
Margreeth95 | 27:aa5ffd7227a5 | 119 | SampleEMGRight.attach(&EMGfilterRight, 0.002); |
Margreeth95 | 18:12250e88037f | 120 | ScopeTimer.attach(&ScopeSend, 0.002); |
Margreeth95 | 27:aa5ffd7227a5 | 121 | MovingAverageLeft.attach(&MovingAverageFilterLeft, 0.005); |
Margreeth95 | 27:aa5ffd7227a5 | 122 | MovingAverageRight.attach(&MovingAverageFilterRight, 0.005); |
laura94 | 29:c43e9f78db37 | 123 | pc.baud(115200); |
laura94 | 29:c43e9f78db37 | 124 | |
Margreeth95 | 30:29c9e9d0e398 | 125 | ledG.write(1), led.write(1), ledB.write(1); |
Margreeth95 | 30:29c9e9d0e398 | 126 | wait(20); |
Margreeth95 | 30:29c9e9d0e398 | 127 | |
laura94 | 29:c43e9f78db37 | 128 | ledG.write(1); |
Margreeth95 | 30:29c9e9d0e398 | 129 | wait(0.2); |
Margreeth95 | 30:29c9e9d0e398 | 130 | ledG.write(0); |
Margreeth95 | 30:29c9e9d0e398 | 131 | wait(0.2); |
Margreeth95 | 30:29c9e9d0e398 | 132 | ledG.write(1); |
Margreeth95 | 30:29c9e9d0e398 | 133 | wait(0.2); |
Margreeth95 | 30:29c9e9d0e398 | 134 | ledG.write(0); |
Margreeth95 | 30:29c9e9d0e398 | 135 | wait(0.2); |
Margreeth95 | 30:29c9e9d0e398 | 136 | ledG.write(1); |
Margreeth95 | 30:29c9e9d0e398 | 137 | wait(0.2); |
laura94 | 29:c43e9f78db37 | 138 | ledG.write(0); |
laura94 | 29:c43e9f78db37 | 139 | wait(2); |
Margreeth95 | 30:29c9e9d0e398 | 140 | Threshold1 = 0.5*EMG_left_MAF; |
Margreeth95 | 30:29c9e9d0e398 | 141 | Threshold2 = 0.2*EMG_left_MAF; |
Margreeth95 | 30:29c9e9d0e398 | 142 | ledG.write(1); |
Margreeth95 | 30:29c9e9d0e398 | 143 | |
Margreeth95 | 30:29c9e9d0e398 | 144 | wait(2); |
Margreeth95 | 30:29c9e9d0e398 | 145 | ledB.write(1); |
Margreeth95 | 30:29c9e9d0e398 | 146 | wait(0.2); |
Margreeth95 | 30:29c9e9d0e398 | 147 | ledB.write(0); |
Margreeth95 | 30:29c9e9d0e398 | 148 | wait(0.2); |
Margreeth95 | 30:29c9e9d0e398 | 149 | ledB.write(1); |
Margreeth95 | 30:29c9e9d0e398 | 150 | wait(0.2); |
Margreeth95 | 30:29c9e9d0e398 | 151 | ledB.write(0); |
Margreeth95 | 30:29c9e9d0e398 | 152 | wait(0.2); |
Margreeth95 | 30:29c9e9d0e398 | 153 | ledB.write(1); |
Margreeth95 | 30:29c9e9d0e398 | 154 | wait(0.2); |
Margreeth95 | 30:29c9e9d0e398 | 155 | ledB.write(0); |
Margreeth95 | 30:29c9e9d0e398 | 156 | wait(2); |
Margreeth95 | 30:29c9e9d0e398 | 157 | Threshold3 = 0.5*EMG_right_MAF; |
Margreeth95 | 30:29c9e9d0e398 | 158 | Threshold4 = 0.2*EMG_right_MAF; |
Margreeth95 | 30:29c9e9d0e398 | 159 | ledB.write(1); |
Margreeth95 | 30:29c9e9d0e398 | 160 | |
Margreeth95 | 30:29c9e9d0e398 | 161 | pc.printf("T1 = %f, T2 = %f, T3 = %f, T4 = %f\n", Threshold1, Threshold2, Threshold3, Threshold4); |
laura94 | 29:c43e9f78db37 | 162 | ledG.write(1); |
laura94 | 29:c43e9f78db37 | 163 | |
Margreeth95 | 19:34456a1effc4 | 164 | while(1) |
Margreeth95 | 19:34456a1effc4 | 165 | { |
Margreeth95 | 30:29c9e9d0e398 | 166 | if (EMG_left_MAF > Threshold1) |
Margreeth95 | 30:29c9e9d0e398 | 167 | { |
Margreeth95 | 30:29c9e9d0e398 | 168 | led.write(0); |
Margreeth95 | 30:29c9e9d0e398 | 169 | } |
Margreeth95 | 30:29c9e9d0e398 | 170 | |
Margreeth95 | 30:29c9e9d0e398 | 171 | else if((EMG_left_MAF > Threshold2) && (led == 0 )) |
Margreeth95 | 19:34456a1effc4 | 172 | { |
Margreeth95 | 19:34456a1effc4 | 173 | led.write(0); |
Margreeth95 | 19:34456a1effc4 | 174 | } |
Margreeth95 | 19:34456a1effc4 | 175 | |
Margreeth95 | 19:34456a1effc4 | 176 | else |
Margreeth95 | 19:34456a1effc4 | 177 | { |
Margreeth95 | 19:34456a1effc4 | 178 | led.write(1); |
Margreeth95 | 19:34456a1effc4 | 179 | } |
Margreeth95 | 19:34456a1effc4 | 180 | } |
vsluiter | 0:32bb76391d89 | 181 | } |