Astrid Schut / Mbed 2 deprecated EMGreader

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?

UserRevisionLine numberNew 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( &notch1_1 );
aschut 1:ecd6dc3c839b 362 highp2.add( &highp2_1 ).add( &highp2_2 ).add( &notch2_1 );
aschut 1:ecd6dc3c839b 363 highp3.add( &highp3_1 ).add( &highp3_2 ).add( &notch3_1 );
aschut 1:ecd6dc3c839b 364 highp4.add( &highp4_1 ).add( &highp4_2 ).add( &notch4_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 }