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
- Committer:
- aschut
- Date:
- 2019-04-16
- Revision:
- 2:f6060b484caf
- Parent:
- 1:ecd6dc3c839b
- Child:
- 3:9cd46de17b01
File content as of revision 2:f6060b484caf:
#include "mbed.h" #include "BiQuad.h" #include "MODSERIAL.h" #include <iostream> #include "MovingAverage.h" #define NSAMPLE 100 DigitalOut led1(LED_GREEN); DigitalOut led2(LED_RED); DigitalOut led3(LED_BLUE); MODSERIAL pc(USBTX, USBRX); // Tickers Ticker sample_ticker; //ticker voor filteren met 1000Hz Ticker threshold_check_ticker; //ticker voor het checken van de threshold met 1000Hz Timer timer_calibration; //timer voor EMG Kalibratie double ts = 0.001; //tijdsstap !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! double calibration_time = 37; //Kalibratie tijd //Input AnalogIn emg1( A1 ); //Duim AnalogIn emg2( A2 ); //Bicep AnalogIn emg3( A3 ); //Dorsaal AnalogIn emg4( A4 ); //Palmair // GLOBALS EMG //Gefilterde EMG signalen volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered; bool thresholdreach1 = false; bool thresholdreach2 = false; bool thresholdreach3 = false; bool thresholdreach4 = false; volatile double temp_highest_emg1 = 0; //Hoogste waarde gevonden tijdens kalibratie volatile double temp_highest_emg2 = 0; volatile double temp_highest_emg3 = 0; volatile double temp_highest_emg4 = 0; //Percentage van de hoogste waarde waar de bovenste treshold gezet moet worden double Duim_p_t = 0.5; double Bicep_p_t = 0.4; double Dorsaal_p_t = 0.6; double Palmair_p_t = 0.5; //Percentage van de hoogste waarde waar de onderste treshold gezet moet worden double Duim_p_tL = 0.5; double Bicep_p_tL = 0.4; double Dorsaal_p_tL = 0.5; double Palmair_p_tL = 0.5; // Waarde bovenste treshold waar het signaal overheen moet om de arm te activeren volatile double threshold1; volatile double threshold2; volatile double threshold3; volatile double threshold4; // Waarde onderste treshold waar het signaal onder moet om de arm te deactiveren volatile double threshold1L; volatile double threshold2L; volatile double threshold3L; volatile double threshold4L; // thresholdreads bools int Duim; int Bicep; int Dorsaal; int Palmair; // EMG OUTPUT int EMGxplus; int EMGxmin ; int EMGyplus; int EMGymin ; //EMG1!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren BiQuadChain highp1; BiQuad highp1_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 ); BiQuad highp1_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 ); BiQuad notch1_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 ); //Lowpass first order cutoff 0.4Hz BiQuad lowp1( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 ); //EMG2!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren BiQuadChain highp2; BiQuad highp2_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 ); BiQuad highp2_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 ); BiQuad notch2_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 ); //Lowpass first order cutoff 0.4Hz BiQuad lowp2( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 ); //EMG3!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren BiQuadChain highp3; BiQuad highp3_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 ); BiQuad highp3_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 ); BiQuad notch3_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 ); //Lowpass first order cutoff 0.4Hz BiQuad lowp3( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 ); //EMG4!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren BiQuadChain highp4; BiQuad highp4_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 ); BiQuad highp4_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 ); BiQuad notch4_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 ); //Lowpass first order cutoff 0.4Hz BiQuad lowp4( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 ); // ~~~~~~~~~~~~~~~~~~~EMG FUNCTIONS~~~~~~~~~~~~~~~~~~ void emgsample() { // EMG signaal lezen double emgread1 = emg1.read(); double emgread2 = emg2.read(); double emgread3 = emg3.read(); double emgread4 = emg4.read(); // Vierde orde highpass filter + notch filter double emg1_highpassed = highp1.step(emgread1); double emg2_highpassed = highp2.step(emgread2); double emg3_highpassed = highp3.step(emgread3); double emg4_highpassed = highp4.step(emgread4); //Rectificatie double emg1_abs = abs(emg1_highpassed); double emg2_abs = abs(emg2_highpassed); double emg3_abs = abs(emg3_highpassed); double emg4_abs = abs(emg4_highpassed); //All EMG abs into lowpass emg1_filtered = lowp1.step(emg1_abs); emg2_filtered = lowp2.step(emg2_abs); emg3_filtered = lowp3.step(emg3_abs); emg4_filtered = lowp4.step(emg4_abs); } void CalibrationEMG() { while(timer_calibration<39) { //Duim if(timer_calibration>0 && timer_calibration<7) { led1=!led1; if(emg1_filtered>temp_highest_emg1) { temp_highest_emg1= emg1_filtered; pc.printf("Highest value Duim= %f \r\n", temp_highest_emg1); } } if(timer_calibration>7 && timer_calibration<10) { led1=0; led2=0; led3=0; } if(timer_calibration>10 && timer_calibration<17) { //Bicep led2=!led2; if(emg2_filtered>temp_highest_emg2) { temp_highest_emg2= emg2_filtered; pc.printf("Highest value Bicep= %f \r\n", temp_highest_emg2); } } if(timer_calibration>17 && timer_calibration<20) { led1=0; led2=0; led3=0; } if(timer_calibration>20 && timer_calibration<27) { //Dorsaal led3=!led3; if(emg3_filtered>temp_highest_emg3) { temp_highest_emg3= emg3_filtered; pc.printf("Highest value Dorsaal= %f \r\n", temp_highest_emg3); } } if(timer_calibration>27 && timer_calibration<30) { led1=0; led2=0; led3=0; } if(timer_calibration>30 && timer_calibration<37) { //Palmair led2=!led2; led3=!led3; if(emg4_filtered>temp_highest_emg4) { temp_highest_emg4= emg4_filtered; pc.printf("Highest value Palmair= %f \r\n", temp_highest_emg4); } } led1=1; led2=1; led3=1; } pc.printf("threshold calculation\r\n"); threshold1 = temp_highest_emg1*Duim_p_t; threshold2 = temp_highest_emg2*Bicep_p_t; threshold3 = temp_highest_emg3*Dorsaal_p_t; threshold4 = temp_highest_emg4*Palmair_p_t; threshold1L = temp_highest_emg1*Duim_p_tL; threshold2L = temp_highest_emg2*Bicep_p_tL; threshold3L = temp_highest_emg3*Dorsaal_p_tL; threshold4L = temp_highest_emg4*Palmair_p_tL; } //Check of emg_filtered boven de threshold is void threshold_check() { // EMG1 Check if (thresholdreach1 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest //bovenste threshold check if(emg1_filtered>threshold1) { Duim = 1; thresholdreach1 = true; } else { Duim= 0; } } else{ //Als emg_filtered boven de bovenste threshold is geweest //onderste threshold check if(emg1_filtered<threshold1L) { Duim = 0; thresholdreach1 = false; } else { Duim= 1; } } // EMG2 Check if (thresholdreach2 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest //bovenste threshold check if(emg2_filtered>threshold2) { Bicep = 1; thresholdreach2 = true; } else { Bicep= 0; } } else{ //Als emg_filtered boven de bovenste threshold is geweest //onderste threshold check if(emg2_filtered<threshold2L) { Bicep = 0; thresholdreach2 = false; } else { Bicep= 1; } } // EMG3 Check if (thresholdreach3 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest //bovenste threshold check if(emg3_filtered>threshold3) { Dorsaal = 1; thresholdreach3 = true; } else { Dorsaal= 0; } } else{ //Als emg_filtered boven de bovenste threshold is geweest //onderste threshold check if(emg3_filtered<threshold3L) { Dorsaal = 0; thresholdreach3 = false; } else { Dorsaal= 1; } } // EMG4 Check if (thresholdreach4 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest //bovenste threshold check if(emg4_filtered>threshold4) { Palmair = 1; thresholdreach4 = true; } else { Palmair= 0; } } else{ //Als emg_filtered boven de bovenste threshold is geweest //onderste threshold check if(emg4_filtered<threshold4L) { Palmair = 0; thresholdreach4 = false; } else { Palmair= 1; } } } Ticker sample_timer; void sample() { pc.printf("Duim Right = %i\r\n", Duim); pc.printf("Bicep Right = %i\r\n",Bicep); pc.printf("Dorsaal Left = %i\r\n", Dorsaal); pc.printf("Palmair Left = %i\r\n", Palmair); if (Duim == 1){ //groen led1 = 0; } else { led1 = 1; } if (Bicep == 1){ //rood led2 = 0; } else { led2 = 1; } if (Dorsaal == 1){ //blauw led3 = 0; } else { led3 = 1; } if (Palmair == 1){ led1 = 0; led2 = 0; led3 = 0; } else { led1 = 1; led1 = 2; led1 = 3; } } int main() { led1 = 1; led2 = 1; led3 = 1; sample_ticker.attach(&emgsample, ts); pc.baud(115200); //BiQuad Chain add highp1.add( &highp1_1 ).add( &highp1_2 ).add( ¬ch1_1 ); highp2.add( &highp2_1 ).add( &highp2_2 ).add( ¬ch2_1 ); highp3.add( &highp3_1 ).add( &highp3_2 ).add( ¬ch3_1 ); highp4.add( &highp4_1 ).add( &highp4_2 ).add( ¬ch4_1 ); temp_highest_emg1 = 0; //highest detected value right Biceps temp_highest_emg2 = 0; temp_highest_emg3 = 0; temp_highest_emg4 = 0; timer_calibration.reset(); timer_calibration.start(); CalibrationEMG(); pc.printf("threshold1 = %i, threshold1L = %f\r\n", threshold1, threshold1L); threshold_check_ticker.attach(&threshold_check, 0.01); led1 = 1; led2 = 1; led3 = 1; sample_timer.attach(&sample, 0.1); pc.printf("sample timer attached\r\n"); timer_calibration.stop(); /*empty loop, sample() is executed periodically*/ while(1) { wait(0.01); } }