EMG measurement in HIDScope working!
Dependencies: HIDScope biquadFilter mbed
Diff: main.cpp
- Revision:
- 0:18cb9a6c4fc1
- Child:
- 1:dea6b70cd991
diff -r 000000000000 -r 18cb9a6c4fc1 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Nov 01 15:54:01 2016 +0000 @@ -0,0 +1,125 @@ +#include "mbed.h" +#include "BiQuad.h" +#define SERIAL_BAUD 115200 // baud rate for serial communication + +// Serial communication with PC +Serial pc(USBTX,USBRX); +AnalogIn emg0( A0 ); +AnalogIn emg1( A1 ); + +Ticker filter_timer, send_timer; +DigitalOut led1(LED_RED); +DigitalOut led2(LED_BLUE); + +volatile bool filter_timer_go=false,send_timer_go=false; + +double EMGright, EMGleft, inR; +int EMGgain=1; +//set initial conditions +double biceps_l = 0; +double biceps_r = 0; +double outRenvelope, outLenvelope; + + +void filter_timer_act(){filter_timer_go=true;}; +void send_timer_act(){send_timer_go=true;}; + +BiQuadChain bcq1; +BiQuadChain bcq2; +// Notch filter wo=50; bw=wo/35 +BiQuad bq1(9.9821e-01,-1.9807e+00,9.9821e-01,-1.9807e+00,9.9642e-01); +// High pass Butterworth filter 2nd order, Fc=10; +BiQuad bq2(9.8239e-01,-1.9648e+00,9.8239e-01,-1.9645e+00,9.6508e-01); +// Low pass Butterworth filter 2nd order, Fc = 8; +BiQuad bq3(5.6248e-05,1.1250e-04,5.6248e-05,-1.9787e+00,9.7890e-01); + + +// In the following: R is used for right arm, L is used for left arm! +void FilteredSample(double &envelopeR, double &envelopeL) +{ + double inLout = emg0.read(); + double inRout = emg1.read(); + + double outRfilter1 = bcq1.step(inRout); + double outRrect= fabs(outRfilter1); + envelopeR = bcq2.step(outRrect); + + double outLfilter1 = bcq1.step(inLout); + double outLrect = fabs(outLfilter1); + envelopeL = bcq2.step(outLrect); + + pc.printf("EMG right = %f\n\r",inRout); + pc.printf("EMG left = %f\n\r",inLout); + pc.printf("envelope EMG right = %f\n\r",envelopeR); + pc.printf("envelope EMG left = %f\n\r",envelopeL); +} + +void sendValues( double outRenvelope, double outLenvelope){ + + biceps_l = (double) outLenvelope * EMGgain; //emg0.read(); //velocity or reference position change, EMG with a gain + biceps_r = (double) outRenvelope * EMGgain; //emg1.read(); + if (biceps_l > 0.2 && biceps_r > 0.2){ + //both arms activated: stamp moves down + pc.printf("Stamp down\n\r"); + pc.printf("right: %f\n\r",biceps_r); + pc.printf("left: %f\n\r",biceps_l); + //wait(0.5); + led1=!led1;//blink purple + led2=!led2; + } + else if (biceps_l > 0.2 && biceps_r <= 0.2){ + //arm 1 activated, move left + pc.printf("Move left\n\r"); + pc.printf("right: %f\n\r",biceps_r); + pc.printf("left: %f\n\r",biceps_l); + //wait(0.5); + led2=1;//off + led1=0;//on red + } + else if (biceps_l <= 0.2 && biceps_r > 0.2){ + //arm 1 activated, move right + pc.printf("Move right\n\r"); + pc.printf("right: %f\n\r",biceps_r); + pc.printf("left: %f\n\r",biceps_l); + //wait(0.5); + led2=0;//on blue + led1=1;//off + } + else{ + wait(0.2); + led1 = 1; + led2 = 1; //off + pc.printf("Nothing...\n\r"); + //wait(0.5); + } + // To indicate that the function is working, the LED is toggled*/ + //led2 = !led2; // blue +} + + + +int main() +{ + pc.baud(SERIAL_BAUD); + led1=1; + led2=1; + led1=0; //red + + bcq1.add(&bq1).add(&bq2); + bcq2.add(&bq3); + + + filter_timer.attach(&filter_timer_act, 0.0004); //2500Hz (same as with filter coefficients on matlab!!! Thus adjust!) + send_timer.attach(&send_timer_act, 0.0004); + pc.printf("\rMain-loop\n\r"); + + while(1) + { + if (filter_timer_go){ + filter_timer_go=false; + FilteredSample(outRenvelope, outLenvelope);} + if (send_timer_go){ + send_timer_go=false; + sendValues(outRenvelope, outLenvelope);} + } +} \ No newline at end of file