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

Committer:
aschut
Date:
Tue Apr 16 12:35:11 2019 +0000
Revision:
2:f6060b484caf
Parent:
1:ecd6dc3c839b
Child:
3:9cd46de17b01
voor Kalibratie aanpassingen

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 0:c9891e1c62f0 156 }
aschut 0:c9891e1c62f0 157 }
aschut 1:ecd6dc3c839b 158 if(timer_calibration>7 && timer_calibration<10) {
aschut 0:c9891e1c62f0 159 led1=0;
aschut 0:c9891e1c62f0 160 led2=0;
aschut 0:c9891e1c62f0 161 led3=0;
aschut 0:c9891e1c62f0 162 }
aschut 1:ecd6dc3c839b 163 if(timer_calibration>10 && timer_calibration<17) { //Bicep
aschut 0:c9891e1c62f0 164 led2=!led2;
aschut 0:c9891e1c62f0 165 if(emg2_filtered>temp_highest_emg2) {
aschut 0:c9891e1c62f0 166 temp_highest_emg2= emg2_filtered;
aschut 1:ecd6dc3c839b 167 pc.printf("Highest value Bicep= %f \r\n", temp_highest_emg2);
aschut 0:c9891e1c62f0 168 }
aschut 0:c9891e1c62f0 169 }
aschut 0:c9891e1c62f0 170 if(timer_calibration>17 && timer_calibration<20) {
aschut 0:c9891e1c62f0 171 led1=0;
aschut 0:c9891e1c62f0 172 led2=0;
aschut 0:c9891e1c62f0 173 led3=0;
aschut 0:c9891e1c62f0 174 }
aschut 1:ecd6dc3c839b 175 if(timer_calibration>20 && timer_calibration<27) { //Dorsaal
aschut 0:c9891e1c62f0 176 led3=!led3;
aschut 0:c9891e1c62f0 177 if(emg3_filtered>temp_highest_emg3) {
aschut 0:c9891e1c62f0 178 temp_highest_emg3= emg3_filtered;
aschut 1:ecd6dc3c839b 179 pc.printf("Highest value Dorsaal= %f \r\n", temp_highest_emg3);
aschut 0:c9891e1c62f0 180 }
aschut 0:c9891e1c62f0 181 }
aschut 0:c9891e1c62f0 182 if(timer_calibration>27 && timer_calibration<30) {
aschut 0:c9891e1c62f0 183 led1=0;
aschut 0:c9891e1c62f0 184 led2=0;
aschut 0:c9891e1c62f0 185 led3=0;
aschut 0:c9891e1c62f0 186 }
aschut 1:ecd6dc3c839b 187 if(timer_calibration>30 && timer_calibration<37) { //Palmair
aschut 0:c9891e1c62f0 188 led2=!led2;
aschut 0:c9891e1c62f0 189 led3=!led3;
aschut 0:c9891e1c62f0 190 if(emg4_filtered>temp_highest_emg4) {
aschut 0:c9891e1c62f0 191 temp_highest_emg4= emg4_filtered;
aschut 1:ecd6dc3c839b 192 pc.printf("Highest value Palmair= %f \r\n", temp_highest_emg4);
aschut 0:c9891e1c62f0 193 }
aschut 0:c9891e1c62f0 194 }
aschut 0:c9891e1c62f0 195 led1=1;
aschut 0:c9891e1c62f0 196 led2=1;
aschut 0:c9891e1c62f0 197 led3=1;
aschut 0:c9891e1c62f0 198
aschut 0:c9891e1c62f0 199 }
aschut 1:ecd6dc3c839b 200 pc.printf("threshold calculation\r\n");
aschut 1:ecd6dc3c839b 201 threshold1 = temp_highest_emg1*Duim_p_t;
aschut 1:ecd6dc3c839b 202 threshold2 = temp_highest_emg2*Bicep_p_t;
aschut 1:ecd6dc3c839b 203 threshold3 = temp_highest_emg3*Dorsaal_p_t;
aschut 1:ecd6dc3c839b 204 threshold4 = temp_highest_emg4*Palmair_p_t;
aschut 1:ecd6dc3c839b 205
aschut 1:ecd6dc3c839b 206 threshold1L = temp_highest_emg1*Duim_p_tL;
aschut 1:ecd6dc3c839b 207 threshold2L = temp_highest_emg2*Bicep_p_tL;
aschut 1:ecd6dc3c839b 208 threshold3L = temp_highest_emg3*Dorsaal_p_tL;
aschut 1:ecd6dc3c839b 209 threshold4L = temp_highest_emg4*Palmair_p_tL;
aschut 0:c9891e1c62f0 210 }
aschut 0:c9891e1c62f0 211
aschut 1:ecd6dc3c839b 212 //Check of emg_filtered boven de threshold is
aschut 0:c9891e1c62f0 213 void threshold_check()
aschut 0:c9891e1c62f0 214 {
aschut 0:c9891e1c62f0 215
aschut 1:ecd6dc3c839b 216 // EMG1 Check
aschut 1:ecd6dc3c839b 217 if (thresholdreach1 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest
aschut 1:ecd6dc3c839b 218 //bovenste threshold check
aschut 0:c9891e1c62f0 219 if(emg1_filtered>threshold1) {
aschut 0:c9891e1c62f0 220 Duim = 1;
aschut 1:ecd6dc3c839b 221 thresholdreach1 = true;
aschut 1:ecd6dc3c839b 222
aschut 0:c9891e1c62f0 223 } else {
aschut 0:c9891e1c62f0 224 Duim= 0;
aschut 0:c9891e1c62f0 225 }
aschut 1:ecd6dc3c839b 226 }
aschut 1:ecd6dc3c839b 227 else{ //Als emg_filtered boven de bovenste threshold is geweest
aschut 1:ecd6dc3c839b 228 //onderste threshold check
aschut 1:ecd6dc3c839b 229 if(emg1_filtered<threshold1L) {
aschut 1:ecd6dc3c839b 230 Duim = 0;
aschut 1:ecd6dc3c839b 231 thresholdreach1 = false;
aschut 1:ecd6dc3c839b 232
aschut 1:ecd6dc3c839b 233 } else {
aschut 1:ecd6dc3c839b 234 Duim= 1;
aschut 1:ecd6dc3c839b 235 }
aschut 1:ecd6dc3c839b 236 }
aschut 1:ecd6dc3c839b 237
aschut 1:ecd6dc3c839b 238 // EMG2 Check
aschut 1:ecd6dc3c839b 239 if (thresholdreach2 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest
aschut 1:ecd6dc3c839b 240 //bovenste threshold check
aschut 0:c9891e1c62f0 241 if(emg2_filtered>threshold2) {
aschut 0:c9891e1c62f0 242 Bicep = 1;
aschut 1:ecd6dc3c839b 243 thresholdreach2 = true;
aschut 0:c9891e1c62f0 244 } else {
aschut 0:c9891e1c62f0 245 Bicep= 0;
aschut 0:c9891e1c62f0 246 }
aschut 0:c9891e1c62f0 247 }
aschut 1:ecd6dc3c839b 248 else{ //Als emg_filtered boven de bovenste threshold is geweest
aschut 1:ecd6dc3c839b 249 //onderste threshold check
aschut 1:ecd6dc3c839b 250 if(emg2_filtered<threshold2L) {
aschut 1:ecd6dc3c839b 251 Bicep = 0;
aschut 1:ecd6dc3c839b 252 thresholdreach2 = false;
aschut 1:ecd6dc3c839b 253
aschut 0:c9891e1c62f0 254 } else {
aschut 1:ecd6dc3c839b 255 Bicep= 1;
aschut 1:ecd6dc3c839b 256 }
aschut 0:c9891e1c62f0 257 }
aschut 0:c9891e1c62f0 258
aschut 1:ecd6dc3c839b 259 // EMG3 Check
aschut 1:ecd6dc3c839b 260 if (thresholdreach3 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest
aschut 1:ecd6dc3c839b 261 //bovenste threshold check
aschut 1:ecd6dc3c839b 262 if(emg3_filtered>threshold3) {
aschut 1:ecd6dc3c839b 263 Dorsaal = 1;
aschut 1:ecd6dc3c839b 264 thresholdreach3 = true;
aschut 1:ecd6dc3c839b 265 } else {
aschut 1:ecd6dc3c839b 266 Dorsaal= 0;
aschut 1:ecd6dc3c839b 267 }
aschut 1:ecd6dc3c839b 268 }
aschut 1:ecd6dc3c839b 269 else{ //Als emg_filtered boven de bovenste threshold is geweest
aschut 1:ecd6dc3c839b 270 //onderste threshold check
aschut 1:ecd6dc3c839b 271 if(emg3_filtered<threshold3L) {
aschut 1:ecd6dc3c839b 272 Dorsaal = 0;
aschut 1:ecd6dc3c839b 273 thresholdreach3 = false;
aschut 1:ecd6dc3c839b 274
aschut 1:ecd6dc3c839b 275 } else {
aschut 1:ecd6dc3c839b 276 Dorsaal= 1;
aschut 1:ecd6dc3c839b 277 }
aschut 1:ecd6dc3c839b 278 }
aschut 0:c9891e1c62f0 279
aschut 1:ecd6dc3c839b 280 // EMG4 Check
aschut 1:ecd6dc3c839b 281 if (thresholdreach4 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest
aschut 1:ecd6dc3c839b 282 //bovenste threshold check
aschut 1:ecd6dc3c839b 283 if(emg4_filtered>threshold4) {
aschut 1:ecd6dc3c839b 284 Palmair = 1;
aschut 1:ecd6dc3c839b 285 thresholdreach4 = true;
aschut 1:ecd6dc3c839b 286 } else {
aschut 1:ecd6dc3c839b 287 Palmair= 0;
aschut 1:ecd6dc3c839b 288 }
aschut 1:ecd6dc3c839b 289 }
aschut 1:ecd6dc3c839b 290 else{ //Als emg_filtered boven de bovenste threshold is geweest
aschut 1:ecd6dc3c839b 291 //onderste threshold check
aschut 1:ecd6dc3c839b 292 if(emg4_filtered<threshold4L) {
aschut 1:ecd6dc3c839b 293 Palmair = 0;
aschut 1:ecd6dc3c839b 294 thresholdreach4 = false;
aschut 1:ecd6dc3c839b 295
aschut 1:ecd6dc3c839b 296 } else {
aschut 1:ecd6dc3c839b 297 Palmair= 1;
aschut 1:ecd6dc3c839b 298 }
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 0:c9891e1c62f0 308
aschut 0:c9891e1c62f0 309
aschut 0:c9891e1c62f0 310 Ticker sample_timer;
aschut 0:c9891e1c62f0 311
aschut 0:c9891e1c62f0 312
aschut 0:c9891e1c62f0 313
aschut 0:c9891e1c62f0 314 void sample()
aschut 0:c9891e1c62f0 315 {
aschut 0:c9891e1c62f0 316 pc.printf("Duim Right = %i\r\n", Duim);
aschut 0:c9891e1c62f0 317 pc.printf("Bicep Right = %i\r\n",Bicep);
aschut 1:ecd6dc3c839b 318 pc.printf("Dorsaal Left = %i\r\n", Dorsaal);
aschut 1:ecd6dc3c839b 319 pc.printf("Palmair Left = %i\r\n", Palmair);
aschut 1:ecd6dc3c839b 320 if (Duim == 1){ //groen
aschut 1:ecd6dc3c839b 321 led1 = 0;
aschut 1:ecd6dc3c839b 322 }
aschut 1:ecd6dc3c839b 323 else {
aschut 1:ecd6dc3c839b 324 led1 = 1;
aschut 1:ecd6dc3c839b 325 }
aschut 1:ecd6dc3c839b 326 if (Bicep == 1){ //rood
aschut 1:ecd6dc3c839b 327 led2 = 0;
aschut 1:ecd6dc3c839b 328 }
aschut 1:ecd6dc3c839b 329 else {
aschut 1:ecd6dc3c839b 330 led2 = 1;
aschut 1:ecd6dc3c839b 331 }
aschut 1:ecd6dc3c839b 332 if (Dorsaal == 1){ //blauw
aschut 1:ecd6dc3c839b 333 led3 = 0;
aschut 1:ecd6dc3c839b 334 }
aschut 1:ecd6dc3c839b 335 else {
aschut 1:ecd6dc3c839b 336 led3 = 1;
aschut 1:ecd6dc3c839b 337 }
aschut 1:ecd6dc3c839b 338 if (Palmair == 1){
aschut 1:ecd6dc3c839b 339 led1 = 0;
aschut 1:ecd6dc3c839b 340 led2 = 0;
aschut 1:ecd6dc3c839b 341 led3 = 0;
aschut 1:ecd6dc3c839b 342 }
aschut 1:ecd6dc3c839b 343 else {
aschut 1:ecd6dc3c839b 344 led1 = 1;
aschut 1:ecd6dc3c839b 345 led1 = 2;
aschut 1:ecd6dc3c839b 346 led1 = 3;
aschut 1:ecd6dc3c839b 347 }
aschut 1:ecd6dc3c839b 348 }
aschut 0:c9891e1c62f0 349
aschut 0:c9891e1c62f0 350
aschut 0:c9891e1c62f0 351 int main()
aschut 0:c9891e1c62f0 352 {
aschut 1:ecd6dc3c839b 353 led1 = 1;
aschut 1:ecd6dc3c839b 354 led2 = 1;
aschut 1:ecd6dc3c839b 355 led3 = 1;
aschut 0:c9891e1c62f0 356 sample_ticker.attach(&emgsample, ts);
aschut 0:c9891e1c62f0 357 pc.baud(115200);
aschut 0:c9891e1c62f0 358
aschut 1:ecd6dc3c839b 359 //BiQuad Chain add
aschut 1:ecd6dc3c839b 360 highp1.add( &highp1_1 ).add( &highp1_2 ).add( &notch1_1 );
aschut 1:ecd6dc3c839b 361 highp2.add( &highp2_1 ).add( &highp2_2 ).add( &notch2_1 );
aschut 1:ecd6dc3c839b 362 highp3.add( &highp3_1 ).add( &highp3_2 ).add( &notch3_1 );
aschut 1:ecd6dc3c839b 363 highp4.add( &highp4_1 ).add( &highp4_2 ).add( &notch4_1 );
aschut 2:f6060b484caf 364
aschut 2:f6060b484caf 365 temp_highest_emg1 = 0; //highest detected value right Biceps
aschut 2:f6060b484caf 366 temp_highest_emg2 = 0;
aschut 2:f6060b484caf 367 temp_highest_emg3 = 0;
aschut 2:f6060b484caf 368 temp_highest_emg4 = 0;
aschut 0:c9891e1c62f0 369
aschut 2:f6060b484caf 370 timer_calibration.reset();
aschut 2:f6060b484caf 371 timer_calibration.start();
aschut 2:f6060b484caf 372 CalibrationEMG();
aschut 2:f6060b484caf 373 pc.printf("threshold1 = %i, threshold1L = %f\r\n", threshold1, threshold1L);
aschut 2:f6060b484caf 374 threshold_check_ticker.attach(&threshold_check, 0.01);
aschut 2:f6060b484caf 375 led1 = 1;
aschut 2:f6060b484caf 376 led2 = 1;
aschut 2:f6060b484caf 377 led3 = 1;
aschut 2:f6060b484caf 378 sample_timer.attach(&sample, 0.1);
aschut 2:f6060b484caf 379 pc.printf("sample timer attached\r\n");
aschut 2:f6060b484caf 380
aschut 2:f6060b484caf 381 timer_calibration.stop();
aschut 2:f6060b484caf 382
aschut 2:f6060b484caf 383
aschut 0:c9891e1c62f0 384
aschut 0:c9891e1c62f0 385 /*empty loop, sample() is executed periodically*/
aschut 0:c9891e1c62f0 386 while(1) {
aschut 0:c9891e1c62f0 387 wait(0.01);
aschut 0:c9891e1c62f0 388 }
aschut 0:c9891e1c62f0 389 }