
Filter for EMG signals The signal will be filtered using a notch, highpass and lowpass filter. The filtered signal will be compared to a preset threshold and according to the strength of the signal the program will perform an action. In this case it will assign a colour to a led.
Dependencies: HIDScope MODSERIAL mbed
Fork of EMGfilter24 by
Diff: main.cpp
- Revision:
- 4:fcada70891c5
- Parent:
- 3:faed8b7f6542
- Child:
- 5:51a28834cd5b
--- a/main.cpp Thu Oct 27 08:14:12 2016 +0000 +++ b/main.cpp Thu Oct 27 14:55:11 2016 +0000 @@ -5,44 +5,93 @@ #include <math.h> MODSERIAL pc(USBTX, USBRX); -Ticker sampleTicker; -Ticker goTicker; -AnalogIn emg0(A0); -AnalogIn emg1(A1); +Ticker sampleTicker;//Ticker that measures the EMG signal every 0,002 seconds and filters it +Ticker goTicker; //Ticker that checks if the signal threshold is reached each time a new filtered EMG signal comes in and determines if the motors are going to rotate +AnalogIn emgl(A1);//Labels are attached to the olimex shields the left tricep should obviously be connected to the should with an L label on it +AnalogIn emgr(A0); DigitalOut ledG(LED_GREEN); DigitalOut ledB(LED_BLUE); DigitalOut ledR(LED_RED); -HIDScope scope(2); +HIDScope scope(2); //scope has two ports for the two EMG signals + +/*coefficients of each filter + lno = left tricep notch filter + lhf = left tricep high pass filter + llf = left tricep lowpass filter + same goes for rno etc. + */ +double lno_b0 = 0.9911; +double lno_b1 = -1.6036; +double lno_b2 = 0.9911; +double lno_a1 = -1.603; +double lno_a2 = 0.9822; + +double rno_b0 = 0.9911; +double rno_b1 = -1.6036; +double rno_b2 = 0.9911; +double rno_a1 = -1.603; +double rno_a2 = 0.9822; -double no_b0 = 0.9911; -double no_b1 = -1.6036; -double no_b2 = 0.9911; -double no_a1 = -1.6036; -double no_a2 = 0.9822; + +double lhf_b0 = 0.9355; +double lhf_b1 = -1.8711; +double lhf_b2 = 0.9355; +double lhf_a1 = -1.8669; +double lhf_a2 = 0.8752; -double hf_b0 = 0.9355; -double hf_b1 = -1.8711; -double hf_b2 = 0.9355; -double hf_a1 = -1.8669; -double hf_a2 = 0.8752; +double rhf_b0 = 0.9355; +double rhf_b1 = -1.8711; +double rhf_b2 = 0.9355; +double rhf_a1 = -1.8669; +double rhf_a2 = 0.8752; + + +double llf_b0 = 8.7656e-5; +double llf_b1 = 1.17531e-4; +double llf_b2 = 8.7656e-5; +double llf_a1 = -1.9733; +double llf_a2 = 0.9737; -double lf_b0 = 8.7656e-5; -double lf_b1 = 1.17531e-4; -double lf_b2 = 8.7656e-5; -double lf_a1 = -1.9733; -double lf_a2 = 0.9737; +double rlf_b0 = 8.7656e-5; +double rlf_b1 = 1.17531e-4; +double rlf_b2 = 8.7656e-5; +double rlf_a1 = -1.9733; +double rlf_a2 = 0.9737; + + +//starting values of the biquads of the corresponding filters +double lno_v1 = 0, lno_v2 = 0; +double lhf_v1 = 0, lhf_v2 = 0; +double llf_v1 = 0, llf_v2 = 0; + +double rno_v1 = 0, rno_v2 = 0; +double rhf_v1 = 0, rhf_v2 = 0; +double rlf_v1 = 0, rlf_v2 = 0; -double no_v1 = 0, no_v2 = 0; -double hf_v1 = 0, hf_v2 = 0; -double lf_v1 = 0, lf_v2 = 0; -double no_y; -double lf_y; -double hf_y; -double rect_y; -int go = 0; -const double threshold_value = 0.13; +/* declaration of the outputs of each biquad. +the output of the previous biquad is the input for the next biquad. +so lno_y goes into lhf_y etc. +*/ +double lno_y; +double lhf_y; +double llf_y; +double lrect_y; +double rno_y; +double rhf_y; +double rlf_y; +double rrect_y; -double biquad_no(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 , +// set the threshold value for the filtered signal +//if the signal exceeds this value the motors will start to rotate +const double threshold_value = 0.05; + +/* declaration of each biquad +The coefficients will be filled in later on in void scopeSend +As said before the input of each biquad is the output of the previous one +The input of the first biquad is the raw EMG signal and the output of the last biquad is the filtered signal. +This is done for both left and right so this makes two chains of 3 biquads */ + +double biquad_lno(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 , const double b1 , const double b2 ) { double v = u - a1*v1 - a2*v2; @@ -52,7 +101,7 @@ return y; } -double biquad_hf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 , +double biquad_lhf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 , const double b1 , const double b2 ) { double v = u - a1*v1 - a2*v2; @@ -62,7 +111,36 @@ return y; } -double biquad_lf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 , +double biquad_llf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 , + const double b1 , const double b2 ) +{ + double v = u - a1*v1 - a2*v2; + double y = b0*v + b1*v1 + b2*v2; + v2 = v1; + v1 = v; + return y; +} +double biquad_rno(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 , + const double b1 , const double b2 ) +{ + double v = u - a1*v1 - a2*v2; + double y = b0*v + b1*v1 + b2*v2; + v2 = v1; + v1 = v; + return y; +} + +double biquad_rhf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 , + const double b1 , const double b2 ) +{ + double v = u - a1*v1 - a2*v2; + double y = b0*v + b1*v1 + b2*v2; + v2 = v1; + v1 = v; + return y; +} + +double biquad_rlf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 , const double b1 , const double b2 ) { double v = u - a1*v1 - a2*v2; @@ -72,46 +150,68 @@ return y; } +/* function that calculates the filtered EMG signal from the raw EMG signal. +So 2 chains of 3 biquads each are calculating the left and the right filtered EMG signal. +After this is calculated, the signals are sent to HIDscope (scope.send) to see what they look like. +The filtered left signal (llf_y) is shown in channel 1, the filtered right signal (rlf_y)is shown in channel 0 (scope.set)*/ void scopeSend(void){ - no_y = biquad_no(emg0.read(), no_v1, no_v2, no_a1, no_a2, no_b0, no_b1, no_b2); - hf_y = biquad_hf(no_y, hf_v1, hf_v2, hf_a1, hf_a2, hf_b0, hf_b1, hf_b2); - rect_y = fabs(hf_y); - lf_y = biquad_lf(rect_y, lf_v1, lf_v2, lf_a1, lf_a2, lf_b0, lf_b1, lf_b2)/0.2; - scope.set(0, emg0.read()); - scope.set(1, lf_y); + lno_y = biquad_lno(emgl.read(), lno_v1, lno_v2, lno_a1, lno_a2, lno_b0, lno_b1, lno_b2); + lhf_y = biquad_lhf(lno_y, lhf_v1, lhf_v2, lhf_a1, lhf_a2, lhf_b0, lhf_b1, lhf_b2); + lrect_y = fabs(lhf_y); + llf_y = biquad_llf(lrect_y, llf_v1, llf_v2, llf_a1, llf_a2, llf_b0, llf_b1, llf_b2)/0.2; + rno_y = biquad_rno(emgr.read(), rno_v1, rno_v2, rno_a1, rno_a2, rno_b0, rno_b1, rno_b2); + rhf_y = biquad_rhf(rno_y, rhf_v1, rhf_v2, rhf_a1, rhf_a2, rhf_b0, rhf_b1, rhf_b2); + rrect_y = fabs(rhf_y); + rlf_y = biquad_rlf(rrect_y, rlf_v1, rlf_v2, rlf_a1, rlf_a2, rlf_b0, rlf_b1, rlf_b2)/0.2; + scope.set(1, llf_y); + scope.set(0, rlf_y); scope.send(); } - -void threshold(double lf_y, const double threshold_value){ - if (lf_y > threshold_value){ - go = !go - } - /* if (lf_y > threshold_value){ - ledB = !ledB; + + //function that compares the filtered EMG signal to the set threshold and determines what colour the led should be + //This led is for feedback purposes only and should obviously be replaced by the motors +void threshold(){ + //If the right signal exceeds the threshold, the led should turn blue + if (rlf_y > threshold_value){ + ledB = 0; + ledR = 1; + ledG = 1; } - else{ - ledB = ledB; + //If the left signal exceeds the threshold, the led should turn red + else if (llf_y > threshold_value){ + ledB = 1; + ledR = 0; + ledG = 1; } + // If both signals exceed the threshold, the led should turn green + else if (rlf_y&&llf_y > threshold_value){ + ledB = 1; + ledR = 1; + ledG = 0; + } + //If no signal exceeds the threshold, the led should be off + else { + ledB=1; + ledG=1; + ledR=1; } -*/ + } + int main(){ + + - /**Attach the 'sample' function to the timer 'sample_timer'. - * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz - */ - // emgSampleTicker.attach(&sample, 0.002); - // empty loop, sample() is executed periodically - + /*Attach the 'sample' function to the timer 'sample_timer'. + this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz + emgSampleTicker.attach(&sample, 0.002); + empty loop, sample() is executed periodically + The same goes for the goTicker. It checks if the threshold is reached at the same rate new a new EMG signal comes in*/ sampleTicker.attach(scopeSend,0.002); goTicker.attach(threshold,0.002); while(1) { - if (go == 0){ - ledB = ledB; - } - else{ - ledB = !ledB; - } + +} } \ No newline at end of file