![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Code om 4 EMG te filteren, thresholds te zetten met kalibratie en met putty te kijken of die thresholds overschreden worden
Dependencies: MovingAverage mbed biquadFilter MODSERIAL
main.cpp@1:ecd6dc3c839b, 2019-04-16 (annotated)
- Committer:
- aschut
- Date:
- Tue Apr 16 11:32:24 2019 +0000
- Revision:
- 1:ecd6dc3c839b
- Parent:
- 0:c9891e1c62f0
- Child:
- 2:f6060b484caf
Werkend met de juiste filters en goede thresholds
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
aschut | 0:c9891e1c62f0 | 1 | #include "mbed.h" |
aschut | 0:c9891e1c62f0 | 2 | #include "BiQuad.h" |
aschut | 0:c9891e1c62f0 | 3 | #include "MODSERIAL.h" |
aschut | 0:c9891e1c62f0 | 4 | #include <iostream> |
aschut | 0:c9891e1c62f0 | 5 | #include "MovingAverage.h" |
aschut | 0:c9891e1c62f0 | 6 | #define NSAMPLE 100 |
aschut | 0:c9891e1c62f0 | 7 | |
aschut | 0:c9891e1c62f0 | 8 | DigitalOut led1(LED_GREEN); |
aschut | 0:c9891e1c62f0 | 9 | DigitalOut led2(LED_RED); |
aschut | 0:c9891e1c62f0 | 10 | DigitalOut led3(LED_BLUE); |
aschut | 0:c9891e1c62f0 | 11 | MODSERIAL pc(USBTX, USBRX); |
aschut | 0:c9891e1c62f0 | 12 | |
aschut | 1:ecd6dc3c839b | 13 | // Tickers |
aschut | 1:ecd6dc3c839b | 14 | Ticker sample_ticker; //ticker voor filteren met 1000Hz |
aschut | 1:ecd6dc3c839b | 15 | Ticker threshold_check_ticker; //ticker voor het checken van de threshold met 1000Hz |
aschut | 1:ecd6dc3c839b | 16 | Timer timer_calibration; //timer voor EMG Kalibratie |
aschut | 1:ecd6dc3c839b | 17 | double ts = 0.001; //tijdsstap !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! |
aschut | 1:ecd6dc3c839b | 18 | double calibration_time = 37; //Kalibratie tijd |
aschut | 0:c9891e1c62f0 | 19 | |
aschut | 0:c9891e1c62f0 | 20 | //Input |
aschut | 1:ecd6dc3c839b | 21 | AnalogIn emg1( A1 ); //Duim |
aschut | 1:ecd6dc3c839b | 22 | AnalogIn emg2( A2 ); //Bicep |
aschut | 1:ecd6dc3c839b | 23 | AnalogIn emg3( A3 ); //Dorsaal |
aschut | 1:ecd6dc3c839b | 24 | AnalogIn emg4( A4 ); //Palmair |
aschut | 0:c9891e1c62f0 | 25 | |
aschut | 0:c9891e1c62f0 | 26 | // GLOBALS EMG |
aschut | 1:ecd6dc3c839b | 27 | //Gefilterde EMG signalen |
aschut | 0:c9891e1c62f0 | 28 | volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered; |
aschut | 0:c9891e1c62f0 | 29 | |
aschut | 1:ecd6dc3c839b | 30 | bool thresholdreach1 = false; |
aschut | 1:ecd6dc3c839b | 31 | bool thresholdreach2 = false; |
aschut | 1:ecd6dc3c839b | 32 | bool thresholdreach3 = false; |
aschut | 1:ecd6dc3c839b | 33 | bool thresholdreach4 = false; |
aschut | 0:c9891e1c62f0 | 34 | |
aschut | 1:ecd6dc3c839b | 35 | volatile double temp_highest_emg1 = 0; //Hoogste waarde gevonden tijdens kalibratie |
aschut | 0:c9891e1c62f0 | 36 | volatile double temp_highest_emg2 = 0; |
aschut | 0:c9891e1c62f0 | 37 | volatile double temp_highest_emg3 = 0; |
aschut | 0:c9891e1c62f0 | 38 | volatile double temp_highest_emg4 = 0; |
aschut | 0:c9891e1c62f0 | 39 | |
aschut | 1:ecd6dc3c839b | 40 | //Percentage van de hoogste waarde waar de bovenste treshold gezet moet worden |
aschut | 1:ecd6dc3c839b | 41 | double Duim_p_t = 0.5; |
aschut | 1:ecd6dc3c839b | 42 | double Bicep_p_t = 0.4; |
aschut | 1:ecd6dc3c839b | 43 | double Dorsaal_p_t = 0.6; |
aschut | 1:ecd6dc3c839b | 44 | double Palmair_p_t = 0.5; |
aschut | 0:c9891e1c62f0 | 45 | |
aschut | 1:ecd6dc3c839b | 46 | //Percentage van de hoogste waarde waar de onderste treshold gezet moet worden |
aschut | 1:ecd6dc3c839b | 47 | double Duim_p_tL = 0.5; |
aschut | 1:ecd6dc3c839b | 48 | double Bicep_p_tL = 0.4; |
aschut | 1:ecd6dc3c839b | 49 | double Dorsaal_p_tL = 0.5; |
aschut | 1:ecd6dc3c839b | 50 | double Palmair_p_tL = 0.5; |
aschut | 1:ecd6dc3c839b | 51 | |
aschut | 1:ecd6dc3c839b | 52 | // Waarde bovenste treshold waar het signaal overheen moet om de arm te activeren |
aschut | 0:c9891e1c62f0 | 53 | volatile double threshold1; |
aschut | 0:c9891e1c62f0 | 54 | volatile double threshold2; |
aschut | 0:c9891e1c62f0 | 55 | volatile double threshold3; |
aschut | 0:c9891e1c62f0 | 56 | volatile double threshold4; |
aschut | 0:c9891e1c62f0 | 57 | |
aschut | 1:ecd6dc3c839b | 58 | // Waarde onderste treshold waar het signaal onder moet om de arm te deactiveren |
aschut | 1:ecd6dc3c839b | 59 | volatile double threshold1L; |
aschut | 1:ecd6dc3c839b | 60 | volatile double threshold2L; |
aschut | 1:ecd6dc3c839b | 61 | volatile double threshold3L; |
aschut | 1:ecd6dc3c839b | 62 | volatile double threshold4L; |
aschut | 1:ecd6dc3c839b | 63 | |
aschut | 0:c9891e1c62f0 | 64 | // thresholdreads bools |
aschut | 0:c9891e1c62f0 | 65 | int Duim; |
aschut | 0:c9891e1c62f0 | 66 | int Bicep; |
aschut | 1:ecd6dc3c839b | 67 | int Dorsaal; |
aschut | 1:ecd6dc3c839b | 68 | int Palmair; |
aschut | 0:c9891e1c62f0 | 69 | |
aschut | 0:c9891e1c62f0 | 70 | // EMG OUTPUT |
aschut | 0:c9891e1c62f0 | 71 | int EMGxplus; |
aschut | 0:c9891e1c62f0 | 72 | int EMGxmin ; |
aschut | 0:c9891e1c62f0 | 73 | int EMGyplus; |
aschut | 0:c9891e1c62f0 | 74 | int EMGymin ; |
aschut | 0:c9891e1c62f0 | 75 | |
aschut | 0:c9891e1c62f0 | 76 | |
aschut | 0:c9891e1c62f0 | 77 | //EMG1!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! |
aschut | 1:ecd6dc3c839b | 78 | //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren |
aschut | 0:c9891e1c62f0 | 79 | BiQuadChain highp1; |
aschut | 1:ecd6dc3c839b | 80 | BiQuad highp1_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 ); |
aschut | 1:ecd6dc3c839b | 81 | BiQuad highp1_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 ); |
aschut | 1:ecd6dc3c839b | 82 | BiQuad notch1_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 ); |
aschut | 0:c9891e1c62f0 | 83 | |
aschut | 1:ecd6dc3c839b | 84 | //Lowpass first order cutoff 0.4Hz |
aschut | 1:ecd6dc3c839b | 85 | BiQuad lowp1( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 ); |
aschut | 0:c9891e1c62f0 | 86 | |
aschut | 0:c9891e1c62f0 | 87 | //EMG2!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! |
aschut | 1:ecd6dc3c839b | 88 | //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren |
aschut | 0:c9891e1c62f0 | 89 | BiQuadChain highp2; |
aschut | 1:ecd6dc3c839b | 90 | BiQuad highp2_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 ); |
aschut | 1:ecd6dc3c839b | 91 | BiQuad highp2_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 ); |
aschut | 1:ecd6dc3c839b | 92 | BiQuad notch2_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 ); |
aschut | 0:c9891e1c62f0 | 93 | |
aschut | 1:ecd6dc3c839b | 94 | //Lowpass first order cutoff 0.4Hz |
aschut | 1:ecd6dc3c839b | 95 | BiQuad lowp2( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 ); |
aschut | 0:c9891e1c62f0 | 96 | |
aschut | 0:c9891e1c62f0 | 97 | //EMG3!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! |
aschut | 1:ecd6dc3c839b | 98 | //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren |
aschut | 0:c9891e1c62f0 | 99 | BiQuadChain highp3; |
aschut | 1:ecd6dc3c839b | 100 | BiQuad highp3_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 ); |
aschut | 1:ecd6dc3c839b | 101 | BiQuad highp3_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 ); |
aschut | 1:ecd6dc3c839b | 102 | BiQuad notch3_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 ); |
aschut | 0:c9891e1c62f0 | 103 | |
aschut | 1:ecd6dc3c839b | 104 | //Lowpass first order cutoff 0.4Hz |
aschut | 1:ecd6dc3c839b | 105 | BiQuad lowp3( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 ); |
aschut | 0:c9891e1c62f0 | 106 | |
aschut | 0:c9891e1c62f0 | 107 | //EMG4!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! |
aschut | 1:ecd6dc3c839b | 108 | //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren |
aschut | 0:c9891e1c62f0 | 109 | BiQuadChain highp4; |
aschut | 1:ecd6dc3c839b | 110 | BiQuad highp4_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 ); |
aschut | 1:ecd6dc3c839b | 111 | BiQuad highp4_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 ); |
aschut | 1:ecd6dc3c839b | 112 | BiQuad notch4_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 ); |
aschut | 0:c9891e1c62f0 | 113 | |
aschut | 1:ecd6dc3c839b | 114 | //Lowpass first order cutoff 0.4Hz |
aschut | 1:ecd6dc3c839b | 115 | BiQuad lowp4( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 ); |
aschut | 0:c9891e1c62f0 | 116 | |
aschut | 0:c9891e1c62f0 | 117 | // ~~~~~~~~~~~~~~~~~~~EMG FUNCTIONS~~~~~~~~~~~~~~~~~~ |
aschut | 0:c9891e1c62f0 | 118 | void emgsample() |
aschut | 0:c9891e1c62f0 | 119 | { |
aschut | 0:c9891e1c62f0 | 120 | |
aschut | 1:ecd6dc3c839b | 121 | // EMG signaal lezen |
aschut | 0:c9891e1c62f0 | 122 | double emgread1 = emg1.read(); |
aschut | 0:c9891e1c62f0 | 123 | double emgread2 = emg2.read(); |
aschut | 0:c9891e1c62f0 | 124 | double emgread3 = emg3.read(); |
aschut | 0:c9891e1c62f0 | 125 | double emgread4 = emg4.read(); |
aschut | 1:ecd6dc3c839b | 126 | |
aschut | 1:ecd6dc3c839b | 127 | // Vierde orde highpass filter + notch filter |
aschut | 0:c9891e1c62f0 | 128 | double emg1_highpassed = highp1.step(emgread1); |
aschut | 0:c9891e1c62f0 | 129 | double emg2_highpassed = highp2.step(emgread2); |
aschut | 0:c9891e1c62f0 | 130 | double emg3_highpassed = highp3.step(emgread3); |
aschut | 0:c9891e1c62f0 | 131 | double emg4_highpassed = highp4.step(emgread4); |
aschut | 0:c9891e1c62f0 | 132 | |
aschut | 1:ecd6dc3c839b | 133 | //Rectificatie |
aschut | 1:ecd6dc3c839b | 134 | double emg1_abs = abs(emg1_highpassed); |
aschut | 1:ecd6dc3c839b | 135 | double emg2_abs = abs(emg2_highpassed); |
aschut | 1:ecd6dc3c839b | 136 | double emg3_abs = abs(emg3_highpassed); |
aschut | 1:ecd6dc3c839b | 137 | double emg4_abs = abs(emg4_highpassed); |
aschut | 0:c9891e1c62f0 | 138 | |
aschut | 0:c9891e1c62f0 | 139 | //All EMG abs into lowpass |
aschut | 1:ecd6dc3c839b | 140 | emg1_filtered = lowp1.step(emg1_abs); |
aschut | 1:ecd6dc3c839b | 141 | emg2_filtered = lowp2.step(emg2_abs); |
aschut | 1:ecd6dc3c839b | 142 | emg3_filtered = lowp3.step(emg3_abs); |
aschut | 1:ecd6dc3c839b | 143 | emg4_filtered = lowp4.step(emg4_abs); |
aschut | 0:c9891e1c62f0 | 144 | |
aschut | 0:c9891e1c62f0 | 145 | } |
aschut | 0:c9891e1c62f0 | 146 | |
aschut | 0:c9891e1c62f0 | 147 | void CalibrationEMG() |
aschut | 0:c9891e1c62f0 | 148 | { |
aschut | 1:ecd6dc3c839b | 149 | |
aschut | 1:ecd6dc3c839b | 150 | while(timer_calibration<39) { //Duim |
aschut | 0:c9891e1c62f0 | 151 | if(timer_calibration>0 && timer_calibration<7) { |
aschut | 0:c9891e1c62f0 | 152 | led1=!led1; |
aschut | 0:c9891e1c62f0 | 153 | if(emg1_filtered>temp_highest_emg1) { |
aschut | 0:c9891e1c62f0 | 154 | temp_highest_emg1= emg1_filtered; |
aschut | 1:ecd6dc3c839b | 155 | pc.printf("Highest value Duim= %f \r\n", temp_highest_emg1); |
aschut | 1:ecd6dc3c839b | 156 | |
aschut | 0:c9891e1c62f0 | 157 | } |
aschut | 0:c9891e1c62f0 | 158 | } |
aschut | 1:ecd6dc3c839b | 159 | if(timer_calibration>7 && timer_calibration<10) { |
aschut | 0:c9891e1c62f0 | 160 | led1=0; |
aschut | 0:c9891e1c62f0 | 161 | led2=0; |
aschut | 0:c9891e1c62f0 | 162 | led3=0; |
aschut | 0:c9891e1c62f0 | 163 | } |
aschut | 1:ecd6dc3c839b | 164 | if(timer_calibration>10 && timer_calibration<17) { //Bicep |
aschut | 0:c9891e1c62f0 | 165 | led2=!led2; |
aschut | 0:c9891e1c62f0 | 166 | if(emg2_filtered>temp_highest_emg2) { |
aschut | 0:c9891e1c62f0 | 167 | temp_highest_emg2= emg2_filtered; |
aschut | 1:ecd6dc3c839b | 168 | pc.printf("Highest value Bicep= %f \r\n", temp_highest_emg2); |
aschut | 0:c9891e1c62f0 | 169 | } |
aschut | 0:c9891e1c62f0 | 170 | } |
aschut | 0:c9891e1c62f0 | 171 | if(timer_calibration>17 && timer_calibration<20) { |
aschut | 0:c9891e1c62f0 | 172 | led1=0; |
aschut | 0:c9891e1c62f0 | 173 | led2=0; |
aschut | 0:c9891e1c62f0 | 174 | led3=0; |
aschut | 0:c9891e1c62f0 | 175 | } |
aschut | 1:ecd6dc3c839b | 176 | if(timer_calibration>20 && timer_calibration<27) { //Dorsaal |
aschut | 0:c9891e1c62f0 | 177 | led3=!led3; |
aschut | 0:c9891e1c62f0 | 178 | if(emg3_filtered>temp_highest_emg3) { |
aschut | 0:c9891e1c62f0 | 179 | temp_highest_emg3= emg3_filtered; |
aschut | 1:ecd6dc3c839b | 180 | pc.printf("Highest value Dorsaal= %f \r\n", temp_highest_emg3); |
aschut | 0:c9891e1c62f0 | 181 | } |
aschut | 0:c9891e1c62f0 | 182 | } |
aschut | 0:c9891e1c62f0 | 183 | if(timer_calibration>27 && timer_calibration<30) { |
aschut | 0:c9891e1c62f0 | 184 | led1=0; |
aschut | 0:c9891e1c62f0 | 185 | led2=0; |
aschut | 0:c9891e1c62f0 | 186 | led3=0; |
aschut | 0:c9891e1c62f0 | 187 | } |
aschut | 1:ecd6dc3c839b | 188 | if(timer_calibration>30 && timer_calibration<37) { //Palmair |
aschut | 0:c9891e1c62f0 | 189 | led2=!led2; |
aschut | 0:c9891e1c62f0 | 190 | led3=!led3; |
aschut | 0:c9891e1c62f0 | 191 | if(emg4_filtered>temp_highest_emg4) { |
aschut | 0:c9891e1c62f0 | 192 | temp_highest_emg4= emg4_filtered; |
aschut | 1:ecd6dc3c839b | 193 | pc.printf("Highest value Palmair= %f \r\n", temp_highest_emg4); |
aschut | 0:c9891e1c62f0 | 194 | } |
aschut | 0:c9891e1c62f0 | 195 | } |
aschut | 0:c9891e1c62f0 | 196 | led1=1; |
aschut | 0:c9891e1c62f0 | 197 | led2=1; |
aschut | 0:c9891e1c62f0 | 198 | led3=1; |
aschut | 0:c9891e1c62f0 | 199 | |
aschut | 0:c9891e1c62f0 | 200 | } |
aschut | 1:ecd6dc3c839b | 201 | pc.printf("threshold calculation\r\n"); |
aschut | 1:ecd6dc3c839b | 202 | threshold1 = temp_highest_emg1*Duim_p_t; |
aschut | 1:ecd6dc3c839b | 203 | threshold2 = temp_highest_emg2*Bicep_p_t; |
aschut | 1:ecd6dc3c839b | 204 | threshold3 = temp_highest_emg3*Dorsaal_p_t; |
aschut | 1:ecd6dc3c839b | 205 | threshold4 = temp_highest_emg4*Palmair_p_t; |
aschut | 1:ecd6dc3c839b | 206 | |
aschut | 1:ecd6dc3c839b | 207 | threshold1L = temp_highest_emg1*Duim_p_tL; |
aschut | 1:ecd6dc3c839b | 208 | threshold2L = temp_highest_emg2*Bicep_p_tL; |
aschut | 1:ecd6dc3c839b | 209 | threshold3L = temp_highest_emg3*Dorsaal_p_tL; |
aschut | 1:ecd6dc3c839b | 210 | threshold4L = temp_highest_emg4*Palmair_p_tL; |
aschut | 0:c9891e1c62f0 | 211 | } |
aschut | 0:c9891e1c62f0 | 212 | |
aschut | 1:ecd6dc3c839b | 213 | //Check of emg_filtered boven de threshold is |
aschut | 0:c9891e1c62f0 | 214 | void threshold_check() |
aschut | 0:c9891e1c62f0 | 215 | { |
aschut | 0:c9891e1c62f0 | 216 | |
aschut | 1:ecd6dc3c839b | 217 | // EMG1 Check |
aschut | 1:ecd6dc3c839b | 218 | if (thresholdreach1 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest |
aschut | 1:ecd6dc3c839b | 219 | //bovenste threshold check |
aschut | 0:c9891e1c62f0 | 220 | if(emg1_filtered>threshold1) { |
aschut | 0:c9891e1c62f0 | 221 | Duim = 1; |
aschut | 1:ecd6dc3c839b | 222 | thresholdreach1 = true; |
aschut | 1:ecd6dc3c839b | 223 | |
aschut | 0:c9891e1c62f0 | 224 | } else { |
aschut | 0:c9891e1c62f0 | 225 | Duim= 0; |
aschut | 0:c9891e1c62f0 | 226 | } |
aschut | 1:ecd6dc3c839b | 227 | } |
aschut | 1:ecd6dc3c839b | 228 | else{ //Als emg_filtered boven de bovenste threshold is geweest |
aschut | 1:ecd6dc3c839b | 229 | //onderste threshold check |
aschut | 1:ecd6dc3c839b | 230 | if(emg1_filtered<threshold1L) { |
aschut | 1:ecd6dc3c839b | 231 | Duim = 0; |
aschut | 1:ecd6dc3c839b | 232 | thresholdreach1 = false; |
aschut | 1:ecd6dc3c839b | 233 | |
aschut | 1:ecd6dc3c839b | 234 | } else { |
aschut | 1:ecd6dc3c839b | 235 | Duim= 1; |
aschut | 1:ecd6dc3c839b | 236 | } |
aschut | 1:ecd6dc3c839b | 237 | } |
aschut | 1:ecd6dc3c839b | 238 | |
aschut | 1:ecd6dc3c839b | 239 | // EMG2 Check |
aschut | 1:ecd6dc3c839b | 240 | if (thresholdreach2 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest |
aschut | 1:ecd6dc3c839b | 241 | //bovenste threshold check |
aschut | 0:c9891e1c62f0 | 242 | if(emg2_filtered>threshold2) { |
aschut | 0:c9891e1c62f0 | 243 | Bicep = 1; |
aschut | 1:ecd6dc3c839b | 244 | thresholdreach2 = true; |
aschut | 0:c9891e1c62f0 | 245 | } else { |
aschut | 0:c9891e1c62f0 | 246 | Bicep= 0; |
aschut | 0:c9891e1c62f0 | 247 | } |
aschut | 0:c9891e1c62f0 | 248 | } |
aschut | 1:ecd6dc3c839b | 249 | else{ //Als emg_filtered boven de bovenste threshold is geweest |
aschut | 1:ecd6dc3c839b | 250 | //onderste threshold check |
aschut | 1:ecd6dc3c839b | 251 | if(emg2_filtered<threshold2L) { |
aschut | 1:ecd6dc3c839b | 252 | Bicep = 0; |
aschut | 1:ecd6dc3c839b | 253 | thresholdreach2 = false; |
aschut | 1:ecd6dc3c839b | 254 | |
aschut | 0:c9891e1c62f0 | 255 | } else { |
aschut | 1:ecd6dc3c839b | 256 | Bicep= 1; |
aschut | 1:ecd6dc3c839b | 257 | } |
aschut | 0:c9891e1c62f0 | 258 | } |
aschut | 0:c9891e1c62f0 | 259 | |
aschut | 1:ecd6dc3c839b | 260 | // EMG3 Check |
aschut | 1:ecd6dc3c839b | 261 | if (thresholdreach3 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest |
aschut | 1:ecd6dc3c839b | 262 | //bovenste threshold check |
aschut | 1:ecd6dc3c839b | 263 | if(emg3_filtered>threshold3) { |
aschut | 1:ecd6dc3c839b | 264 | Dorsaal = 1; |
aschut | 1:ecd6dc3c839b | 265 | thresholdreach3 = true; |
aschut | 1:ecd6dc3c839b | 266 | } else { |
aschut | 1:ecd6dc3c839b | 267 | Dorsaal= 0; |
aschut | 1:ecd6dc3c839b | 268 | } |
aschut | 1:ecd6dc3c839b | 269 | } |
aschut | 1:ecd6dc3c839b | 270 | else{ //Als emg_filtered boven de bovenste threshold is geweest |
aschut | 1:ecd6dc3c839b | 271 | //onderste threshold check |
aschut | 1:ecd6dc3c839b | 272 | if(emg3_filtered<threshold3L) { |
aschut | 1:ecd6dc3c839b | 273 | Dorsaal = 0; |
aschut | 1:ecd6dc3c839b | 274 | thresholdreach3 = false; |
aschut | 1:ecd6dc3c839b | 275 | |
aschut | 1:ecd6dc3c839b | 276 | } else { |
aschut | 1:ecd6dc3c839b | 277 | Dorsaal= 1; |
aschut | 1:ecd6dc3c839b | 278 | } |
aschut | 1:ecd6dc3c839b | 279 | } |
aschut | 0:c9891e1c62f0 | 280 | |
aschut | 1:ecd6dc3c839b | 281 | // EMG4 Check |
aschut | 1:ecd6dc3c839b | 282 | if (thresholdreach4 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest |
aschut | 1:ecd6dc3c839b | 283 | //bovenste threshold check |
aschut | 1:ecd6dc3c839b | 284 | if(emg4_filtered>threshold4) { |
aschut | 1:ecd6dc3c839b | 285 | Palmair = 1; |
aschut | 1:ecd6dc3c839b | 286 | thresholdreach4 = true; |
aschut | 1:ecd6dc3c839b | 287 | } else { |
aschut | 1:ecd6dc3c839b | 288 | Palmair= 0; |
aschut | 1:ecd6dc3c839b | 289 | } |
aschut | 1:ecd6dc3c839b | 290 | } |
aschut | 1:ecd6dc3c839b | 291 | else{ //Als emg_filtered boven de bovenste threshold is geweest |
aschut | 1:ecd6dc3c839b | 292 | //onderste threshold check |
aschut | 1:ecd6dc3c839b | 293 | if(emg4_filtered<threshold4L) { |
aschut | 1:ecd6dc3c839b | 294 | Palmair = 0; |
aschut | 1:ecd6dc3c839b | 295 | thresholdreach4 = false; |
aschut | 1:ecd6dc3c839b | 296 | |
aschut | 1:ecd6dc3c839b | 297 | } else { |
aschut | 1:ecd6dc3c839b | 298 | Palmair= 1; |
aschut | 1:ecd6dc3c839b | 299 | } |
aschut | 1:ecd6dc3c839b | 300 | } |
aschut | 1:ecd6dc3c839b | 301 | |
aschut | 1:ecd6dc3c839b | 302 | } |
aschut | 1:ecd6dc3c839b | 303 | |
aschut | 1:ecd6dc3c839b | 304 | |
aschut | 1:ecd6dc3c839b | 305 | |
aschut | 1:ecd6dc3c839b | 306 | |
aschut | 1:ecd6dc3c839b | 307 | |
aschut | 1:ecd6dc3c839b | 308 | |
aschut | 0:c9891e1c62f0 | 309 | |
aschut | 0:c9891e1c62f0 | 310 | |
aschut | 0:c9891e1c62f0 | 311 | Ticker sample_timer; |
aschut | 0:c9891e1c62f0 | 312 | |
aschut | 0:c9891e1c62f0 | 313 | |
aschut | 0:c9891e1c62f0 | 314 | |
aschut | 0:c9891e1c62f0 | 315 | void sample() |
aschut | 0:c9891e1c62f0 | 316 | { |
aschut | 0:c9891e1c62f0 | 317 | pc.printf("Duim Right = %i\r\n", Duim); |
aschut | 0:c9891e1c62f0 | 318 | pc.printf("Bicep Right = %i\r\n",Bicep); |
aschut | 1:ecd6dc3c839b | 319 | pc.printf("Dorsaal Left = %i\r\n", Dorsaal); |
aschut | 1:ecd6dc3c839b | 320 | pc.printf("Palmair Left = %i\r\n", Palmair); |
aschut | 1:ecd6dc3c839b | 321 | if (Duim == 1){ //groen |
aschut | 1:ecd6dc3c839b | 322 | led1 = 0; |
aschut | 1:ecd6dc3c839b | 323 | } |
aschut | 1:ecd6dc3c839b | 324 | else { |
aschut | 1:ecd6dc3c839b | 325 | led1 = 1; |
aschut | 1:ecd6dc3c839b | 326 | } |
aschut | 1:ecd6dc3c839b | 327 | if (Bicep == 1){ //rood |
aschut | 1:ecd6dc3c839b | 328 | led2 = 0; |
aschut | 1:ecd6dc3c839b | 329 | } |
aschut | 1:ecd6dc3c839b | 330 | else { |
aschut | 1:ecd6dc3c839b | 331 | led2 = 1; |
aschut | 1:ecd6dc3c839b | 332 | } |
aschut | 1:ecd6dc3c839b | 333 | if (Dorsaal == 1){ //blauw |
aschut | 1:ecd6dc3c839b | 334 | led3 = 0; |
aschut | 1:ecd6dc3c839b | 335 | } |
aschut | 1:ecd6dc3c839b | 336 | else { |
aschut | 1:ecd6dc3c839b | 337 | led3 = 1; |
aschut | 1:ecd6dc3c839b | 338 | } |
aschut | 1:ecd6dc3c839b | 339 | if (Palmair == 1){ |
aschut | 1:ecd6dc3c839b | 340 | led1 = 0; |
aschut | 1:ecd6dc3c839b | 341 | led2 = 0; |
aschut | 1:ecd6dc3c839b | 342 | led3 = 0; |
aschut | 1:ecd6dc3c839b | 343 | } |
aschut | 1:ecd6dc3c839b | 344 | else { |
aschut | 1:ecd6dc3c839b | 345 | led1 = 1; |
aschut | 1:ecd6dc3c839b | 346 | led1 = 2; |
aschut | 1:ecd6dc3c839b | 347 | led1 = 3; |
aschut | 1:ecd6dc3c839b | 348 | } |
aschut | 1:ecd6dc3c839b | 349 | } |
aschut | 0:c9891e1c62f0 | 350 | |
aschut | 0:c9891e1c62f0 | 351 | |
aschut | 0:c9891e1c62f0 | 352 | int main() |
aschut | 0:c9891e1c62f0 | 353 | { |
aschut | 1:ecd6dc3c839b | 354 | led1 = 1; |
aschut | 1:ecd6dc3c839b | 355 | led2 = 1; |
aschut | 1:ecd6dc3c839b | 356 | led3 = 1; |
aschut | 0:c9891e1c62f0 | 357 | sample_ticker.attach(&emgsample, ts); |
aschut | 0:c9891e1c62f0 | 358 | pc.baud(115200); |
aschut | 0:c9891e1c62f0 | 359 | |
aschut | 1:ecd6dc3c839b | 360 | //BiQuad Chain add |
aschut | 1:ecd6dc3c839b | 361 | highp1.add( &highp1_1 ).add( &highp1_2 ).add( ¬ch1_1 ); |
aschut | 1:ecd6dc3c839b | 362 | highp2.add( &highp2_1 ).add( &highp2_2 ).add( ¬ch2_1 ); |
aschut | 1:ecd6dc3c839b | 363 | highp3.add( &highp3_1 ).add( &highp3_2 ).add( ¬ch3_1 ); |
aschut | 1:ecd6dc3c839b | 364 | highp4.add( &highp4_1 ).add( &highp4_2 ).add( ¬ch4_1 ); |
aschut | 1:ecd6dc3c839b | 365 | |
aschut | 1:ecd6dc3c839b | 366 | // Ticker voor EMG uitlezen |
aschut | 0:c9891e1c62f0 | 367 | |
aschut | 0:c9891e1c62f0 | 368 | |
aschut | 1:ecd6dc3c839b | 369 | temp_highest_emg1 = 0; //highest detected value right Biceps |
aschut | 0:c9891e1c62f0 | 370 | temp_highest_emg2 = 0; |
aschut | 0:c9891e1c62f0 | 371 | temp_highest_emg3 = 0; |
aschut | 0:c9891e1c62f0 | 372 | temp_highest_emg4 = 0; |
aschut | 0:c9891e1c62f0 | 373 | |
aschut | 0:c9891e1c62f0 | 374 | timer_calibration.reset(); |
aschut | 0:c9891e1c62f0 | 375 | timer_calibration.start(); |
aschut | 1:ecd6dc3c839b | 376 | CalibrationEMG(); |
aschut | 1:ecd6dc3c839b | 377 | pc.printf("threshold1 = %i, threshold1L = %f\r\n", threshold1, threshold1L); |
aschut | 1:ecd6dc3c839b | 378 | threshold_check_ticker.attach(&threshold_check, 0.01); |
aschut | 1:ecd6dc3c839b | 379 | led1 = 1; |
aschut | 1:ecd6dc3c839b | 380 | led2 = 1; |
aschut | 1:ecd6dc3c839b | 381 | led3 = 1; |
aschut | 1:ecd6dc3c839b | 382 | sample_timer.attach(&sample, 0.1); |
aschut | 1:ecd6dc3c839b | 383 | pc.printf("sample timer attached\r\n"); |
aschut | 0:c9891e1c62f0 | 384 | |
aschut | 1:ecd6dc3c839b | 385 | timer_calibration.stop(); |
aschut | 0:c9891e1c62f0 | 386 | |
aschut | 0:c9891e1c62f0 | 387 | |
aschut | 0:c9891e1c62f0 | 388 | |
aschut | 0:c9891e1c62f0 | 389 | /*empty loop, sample() is executed periodically*/ |
aschut | 0:c9891e1c62f0 | 390 | while(1) { |
aschut | 0:c9891e1c62f0 | 391 | wait(0.01); |
aschut | 0:c9891e1c62f0 | 392 | } |
aschut | 0:c9891e1c62f0 | 393 | } |