BMT M9 groep 7 / Mbed 2 deprecated EMG_final

Het totale EMG script

Dependencies:   HIDScope MODSERIAL biquadFilter mbed

Fork of EMG_totaal by Margreeth de Breij

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?

UserRevisionLine numberNew 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 }