![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
a biquad working
Dependencies: HIDScope mbed QEI
Diff: main.cpp
- Revision:
- 2:241b572207fb
- Parent:
- 1:ee877d784c40
- Child:
- 3:2f75bb307da3
--- a/main.cpp Tue Oct 27 10:13:02 2015 +0000 +++ b/main.cpp Tue Oct 27 10:56:41 2015 +0000 @@ -30,14 +30,39 @@ double filter_signal_hid = 0; //double input_right = 0; + +//*** making the v for everything and conquer the world*** + +//for Notchfilter +double notch_v11=0; +double notch_v21=0; +double notch_v12=0; +double notch_v22=0; +double notch_v13=0; +double notch_v23=0; + +//for highpass filter +double high_v11=0; +double high_v21=0; +double high_v12=0; +double high_v22=0; +double high_v13=0; +double high_v23=0; + +// for lowpasfilter double low_v11=0; double low_v21=0; double low_v12=0; double low_v22=0; double low_v13=0; double low_v23=0; -//double v1_right=0; -//double v2_right=0; + +// for moving average +double n1 = 0; +double n2 = 0; +double n3 = 0; +double n4 = 0; +double n5 = 0; double filter_left; double filter_right; @@ -55,7 +80,64 @@ return y; } +double moving_average(double y, double &n1, double &n2, double &n3, double &n4, double &n5) +{ + double average = (y + n1 + n2 +n3 + n4 + n5)/5; + n5 = n4; + n4 = n3; + n3 = n2; + n2 = n1; + n1 = y; + + return average; +} +//Specifying filter coefficients highpass + +/* notch filter with 3 cascaded biquads*/ +//first notch biquad +const double notch1_a1 = -1.55951422433; +const double notch1_a2 = 0.92705680308; +const double notch1_b0 = 1.00000000000; +const double notch1_b1 = -1.61854515325; +const double notch1_b2 = 1.00000000000; + +//second notch biquad +const double notch2_a1 = -1.54767435801; +const double notch2_a2 = 0.96124842048; +const double notch2_b0 = 1.00000000000; +const double notch2_b1 = -1.61854515325; +const double notch2_b2 = 1.00000000000; + +//third notch biquad +const double notch3_a1 = -1.62600366964; +const double notch3_a2 = 0.96453460373; +const double notch3_b0 = 1.00000000000; +const double notch3_b1 = -1.61854515325; +const double notch3_b2 = 1.00000000000; + +/* high pass filter consists of three cascaded biquads + blow coefficients for those three biquads */ +//first high pass biquad +const double highp1_a1 = -0.67538034389; +const double highp1_a2 = 0.12769255668; +const double highp1_b0 = 1.00000000000; +const double highp1_b1 = -2.00000000000; +const double highp1_b2 = 1.00000000000; + +//second high pass biquad +const double highp2_a1 = -0.76475499450; +const double highp2_a2 = 0.27692273367; +const double highp2_b0 = 1.00000000000; +const double highp2_b1 = -2.00000000000; +const double highp2_b2 = 1.00000000000; + +//third high pass biquad +const double highp3_a1 = -0.99216561242; +const double highp3_a2 = 0.65663360837; +const double highp3_b0 = 1.00000000000; +const double highp3_b1 = -2.00000000000; +const double highp3_b2 = 1.00000000000; /* lowpass filter consists of three cascaded biquads below the coefficients for those three biquads */ @@ -86,24 +168,6 @@ -//highpass - - - -/* -//rectifier -double rectify(double y6) -{ - y6 = fabs(y6); - return y6; -} -*/ -//lowpass - - - - - // double y8 = biquad(y7, v1, v2, lowp2_a1, lowp2_a2, lowp2_b0, lowp2_b1, lowp2_b2); @@ -113,14 +177,31 @@ double Filteren() { input = analog_emg_left.read(); - //input = input-0.45; //FIRST SUBTRACT MEAN THEN FILTER + input = input-0.45; //FIRST SUBTRACT MEAN THEN FILTER //input_right = analog_emg_right.read(); - double y7 = biquad(input, low_v11, low_v21, lowp1_a1, lowp1_a2, lowp1_b0, lowp1_b1, lowp1_b2); - double y8 = biquad(y7, low_v12, low_v22, lowp2_a1, lowp2_a2, lowp2_b0, lowp2_b1, lowp2_b2); - double filter_signal = biquad(y8, low_v13, low_v23, lowp3_a1, lowp3_a2, lowp3_b0, lowp3_b1, lowp3_b2); - //filter_right = filter(input_right, v1_right, v2_right); + + // notch filter + double y1 = biquad(input, notch_v11, notch_v21, notch1_a1, notch1_a2, notch1_b0, notch1_b1, notch1_b2); + double y2 = biquad(y1, notch_v12, notch_v22, notch2_a1, notch2_a2, notch2_b0, notch2_b1, notch2_b2); + double y3 = biquad(y2, notch_v13, notch_v23, notch3_a1, notch3_a2, notch3_b0, notch3_b1, notch3_b2); -return(filter_signal); + //higpass filter + double y4 = biquad(y3, high_v11, high_v21, highp1_a1, highp1_a2, highp1_b0, highp1_b1, highp1_b2); + double y5 = biquad(y4, high_v12, high_v22, highp2_a1, highp2_a2, highp2_b0, highp2_b1, highp2_b2); + double y6 = biquad(y5, high_v13, high_v23, highp3_a1, highp3_a2, highp3_b0, highp3_b1, highp3_b2); + + //rectivier + double y7 = fabs(y6); + + //lowpas filter cascade + double y8 = biquad(y7, low_v11, low_v21, lowp1_a1, lowp1_a2, lowp1_b0, lowp1_b1, lowp1_b2); + double y9 = biquad(y8, low_v12, low_v22, lowp2_a1, lowp2_a2, lowp2_b0, lowp2_b1, lowp2_b2); + double y10= biquad(y9, low_v13, low_v23, lowp3_a1, lowp3_a2, lowp3_b0, lowp3_b1, lowp3_b2); + + // moving average + double filter_signal = moving_average(y10,n1,n2,n3,n4,n5); + + return(filter_signal); } void HIDScope_kijken() { @@ -132,8 +213,8 @@ { HIDScope_timer.attach(&Go_flag_HIDScope, 0.002); Filteren_timer.attach(&Go_flag_filteren,0.004); - while(1){ - if(Flag_filteren) { + while(1) { + if(Flag_filteren) { Flag_filteren = false; filter_signal_hid = Filteren(); } @@ -142,5 +223,5 @@ Flag_HIDScope = false; HIDScope_kijken(); } - } + } } \ No newline at end of file