![](/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
- Committer:
- aschut
- Date:
- 2019-04-16
- Revision:
- 0:c9891e1c62f0
- Child:
- 1:ecd6dc3c839b
File content as of revision 0:c9891e1c62f0:
#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); //MovingAverage MovingAverage <float>Movag_1(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above MovingAverage <float>Movag_2(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above MovingAverage <float>Movag_3(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above MovingAverage <float>Movag_4(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above //EMG tickers, these tickers are called in the mainscript with fsample 500Hz, also sends to HIDscope with same fsample Ticker sample_ticker; //ticker for filtering pref. with 1000Hz, define in tick.attach Ticker threshold_check_ticker; Timer t; //timer try out for Astrid Timer timer_calibration; //timer for EMG calibration //Input AnalogIn emg1( A1 ); AnalogIn emg2( A2 ); AnalogIn emg3( A3 ); AnalogIn emg4( A4 ); // GLOBALS EMG //Filtered EMG signals from the end of the chains volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered; int i = 0; //Define doubles for calibration and ticker double ts = 0.001; //tijdsstap double calibration_time = 55; //time EMG calibration should take volatile double temp_highest_emg1 = 0; //highest detected value right biceps volatile double temp_highest_emg2 = 0; volatile double temp_highest_emg3 = 0; volatile double temp_highest_emg4 = 0; //Doubles for calculation threshold double duim_p_t = 0.4; //set threshold at percentage of highest value double bicep_p_t = 0.4; // double tricep_p_t = 0.4; //set threshold at percentage of highest value double onderarm_p_t = 0.4; volatile double threshold1; volatile double threshold2; volatile double threshold3; volatile double threshold4; // thresholdreads bools int Duim; int Bicep; int Tricep; int Onderarm; // EMG OUTPUT int EMGxplus; int EMGxmin ; int EMGyplus; int EMGymin ; //EMG1!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! //Highpass BiQuadChain highp1; BiQuad highp1_1( 9.21171e-01, -1.84234e+00, 9.21171e-01, -1.88661e+00, 8.90340e-01 ); BiQuad highp1_2( 1.00000e+00, -2.00000e+00, 1.00000e+00, -1.94922e+00, 9.53070e-01 ); //Notch BiQuadChain notch1; BiQuad notch1_1( 9.56543e-01, -1.82035e+00, 9.56543e-01, -1.84459e+00, 9.53626e-01 ); BiQuad notch1_2( 1.00000e+00, -1.90305e+00, 1.00000e+00, -1.87702e+00, 9.59471e-01 ); //Lowpass 4th order cutoff 3Hz BiQuadChain lowp1; BiQuad lowp1_1( 7.69910e-09, 1.53982e-08, 7.69910e-09, -1.96542e+00, 9.65769e-01 ); BiQuad lowp1_2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.98532e+00, 9.85677e-01 ); //EMG2!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! //Highpass BiQuadChain highp2; BiQuad highp2_1( 9.21171e-01, -1.84234e+00, 9.21171e-01, -1.88661e+00, 8.90340e-01 ); BiQuad highp2_2( 1.00000e+00, -2.00000e+00, 1.00000e+00, -1.94922e+00, 9.53070e-01 ); //Notch BiQuadChain notch2; BiQuad notch2_1( 9.56543e-01, -1.82035e+00, 9.56543e-01, -1.84459e+00, 9.53626e-01 ); BiQuad notch2_2( 1.00000e+00, -1.90305e+00, 1.00000e+00, -1.87702e+00, 9.59471e-01 ); //Lowpass 4th order cutoff 3Hz BiQuadChain lowp2; BiQuad lowp2_1( 7.69910e-09, 1.53982e-08, 7.69910e-09, -1.96542e+00, 9.65769e-01 ); BiQuad lowp2_2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.98532e+00, 9.85677e-01 ); //EMG3!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! //Highpass BiQuadChain highp3; BiQuad highp3_1( 9.21171e-01, -1.84234e+00, 9.21171e-01, -1.88661e+00, 8.90340e-01 ); BiQuad highp3_2( 1.00000e+00, -2.00000e+00, 1.00000e+00, -1.94922e+00, 9.53070e-01 ); //Notch BiQuadChain notch3; BiQuad notch3_1( 9.56543e-01, -1.82035e+00, 9.56543e-01, -1.84459e+00, 9.53626e-01 ); BiQuad notch3_2( 1.00000e+00, -1.90305e+00, 1.00000e+00, -1.87702e+00, 9.59471e-01 ); //Lowpass 4th order cutoff 3Hz BiQuadChain lowp3; BiQuad lowp3_1( 7.69910e-09, 1.53982e-08, 7.69910e-09, -1.96542e+00, 9.65769e-01 ); BiQuad lowp3_2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.98532e+00, 9.85677e-01 ); //EMG4!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! //Highpass BiQuadChain highp4; BiQuad highp4_1( 9.21171e-01, -1.84234e+00, 9.21171e-01, -1.88661e+00, 8.90340e-01 ); BiQuad highp4_2( 1.00000e+00, -2.00000e+00, 1.00000e+00, -1.94922e+00, 9.53070e-01 ); //Notch BiQuadChain notch4; BiQuad notch4_1( 9.56543e-01, -1.82035e+00, 9.56543e-01, -1.84459e+00, 9.53626e-01 ); BiQuad notch4_2( 1.00000e+00, -1.90305e+00, 1.00000e+00, -1.87702e+00, 9.59471e-01 ); //Lowpass 4th order cutoff 3Hz BiQuadChain lowp4; BiQuad lowp4_1( 7.69910e-09, 1.53982e-08, 7.69910e-09, -1.96542e+00, 9.65769e-01 ); BiQuad lowp4_2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.98532e+00, 9.85677e-01 ); // ~~~~~~~~~~~~~~~~~~~EMG FUNCTIONS~~~~~~~~~~~~~~~~~~ void emgsample() { //All EMG signal through Highpass double emgread1 = emg1.read(); double emgread2 = emg2.read(); double emgread3 = emg3.read(); double emgread4 = emg4.read(); double emg1_highpassed = highp1.step(emgread1); double emg2_highpassed = highp2.step(emgread2); double emg3_highpassed = highp3.step(emgread3); double emg4_highpassed = highp4.step(emgread4); //All EMG highpassed through Notch double emg1_notched = notch1.step(emg1_highpassed); double emg2_notched = notch2.step(emg2_highpassed); double emg3_notched = notch3.step(emg3_highpassed); double emg4_notched = notch4.step(emg4_highpassed); //All EMG notched rectify double emg1_abs = abs(emg1_notched); double emg2_abs = abs(emg2_notched); double emg3_abs = abs(emg3_notched); double emg4_abs = abs(emg4_notched); //All EMG abs into lowpass double emg1_lp = lowp1.step(emg1_abs); double emg2_lp = lowp2.step(emg2_abs); double emg3_lp = lowp3.step(emg3_abs); double emg4_lp = lowp4.step(emg4_abs); Movag_1.Insert(emg1_lp); Movag_2.Insert(emg2_lp); Movag_3.Insert(emg3_lp); Movag_4.Insert(emg4_lp); emg1_filtered = Movag_1.GetAverage(); emg2_filtered = Movag_2.GetAverage(); emg3_filtered = Movag_3.GetAverage(); emg4_filtered = Movag_4.GetAverage(); } void CalibrationEMG() { //static float samples = calibration_time/ts; while(timer_calibration<37) { if(timer_calibration>0 && timer_calibration<7) { led1=!led1; if(emg1_filtered>temp_highest_emg1) { temp_highest_emg1= emg1_filtered; pc.printf("Temp1 = %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) { led2=!led2; if(emg2_filtered>temp_highest_emg2) { temp_highest_emg2= emg2_filtered; pc.printf("Temp2 = %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) { led3=!led3; if(emg3_filtered>temp_highest_emg3) { temp_highest_emg3= emg3_filtered; pc.printf("Temp3 = %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) { led2=!led2; led3=!led3; if(emg4_filtered>temp_highest_emg4) { temp_highest_emg4= emg4_filtered; pc.printf("Temp4 = %f \r\n",temp_highest_emg4); } } led1=1; led2=1; led3=1; } pc.printf("Highest value Duim= %f \r\n", temp_highest_emg1); pc.printf("Highest value Bicep= %f \r\n", temp_highest_emg2); pc.printf("Highest value Tricep= %f \r\n", temp_highest_emg3); pc.printf("Highest value Onderarm= %f \r\n", temp_highest_emg4); threshold1 = temp_highest_emg1*duim_p_t; //Right Biceps threshold2 = temp_highest_emg2*bicep_p_t; //Right Triceps threshold3 = temp_highest_emg3*tricep_p_t; //Left Biceps threshold4 = temp_highest_emg4*onderarm_p_t; //Left Triceps } //Check if emg_filtered has reached their threshold void threshold_check() { //EMG1 threshold check if(emg1_filtered>threshold1) { Duim = 1; led1 = !led1; } else { Duim= 0; } //EMG2 threshold check if(emg2_filtered>threshold2) { Bicep = 1; led2 = !led2; } else { Bicep= 0; } //EMG3 threshold check if(emg3_filtered>threshold3) { Tricep = 1; led3 = !led3; } else { Tricep= 0; } //EMG4 threshold check if(emg4_filtered>threshold4) { Onderarm = 1; led1 = !led1; led2 = !led1; led3 = !led1; } else { Onderarm= 0; } } Ticker sample_timer; void sample() { pc.printf("Duim Right = %i\r\n", Duim); pc.printf("Bicep Right = %i\r\n",Bicep); pc.printf("tricep Left = %i\r\n", Tricep); pc.printf("onderarm Left = %i\r\n", Onderarm); wait(0.01); } int main() { sample_ticker.attach(&emgsample, ts); pc.baud(115200); //BiQuad Chain add highp1.add( &highp1_1 ).add( &highp1_2 ); notch1.add( ¬ch1_1 ).add( ¬ch1_2 ); lowp1.add( &lowp1_1 ).add(&lowp1_2); highp2.add( &highp2_1 ).add( &highp2_2 ); notch2.add( ¬ch2_1 ).add( ¬ch2_2 ); lowp2.add( &lowp2_1 ).add(&lowp2_2); highp3.add( &highp3_1 ).add( &highp3_2 ); notch3.add( ¬ch3_1 ).add( ¬ch3_2 ); lowp3.add( &lowp3_1 ).add(&lowp3_2); highp4.add( &highp4_1 ).add( &highp4_2 ); notch4.add( ¬ch4_1 ).add( ¬ch4_2 ); lowp4.add( &lowp4_1 ).add(&lowp4_2); 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(); threshold_check_ticker.attach(threshold_check, 0.1); sample_timer.attach(&sample, 0.1); //sample_ticker.detach(); timer_calibration.stop(); /*empty loop, sample() is executed periodically*/ while(1) { wait(0.01); } }