Biorobotics
/
piano_robot
newest version,
Diff: read_filter_new.cpp
- Revision:
- 3:11c2175b4478
diff -r b90bb3ef67cf -r 11c2175b4478 read_filter_new.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/read_filter_new.cpp Mon Oct 26 11:42:17 2015 +0000 @@ -0,0 +1,122 @@ +#include "mbed.h" +#include "read_filter_emg.h" +//included for fabs() function +#include <math.h> + +//input (u) = analogin + +AnalogIn(/*pin-name*/) = &aI; +input = aI.read(); + +double v1=0; +double v2=0; + +//general biquad filter that can be called in all the filter functions +double biquad(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; + //values of v2 and v1 are updated, as they are passed by reference + //they update globally + v2 = v1; v1 = v; + return y; +} + +//Specifying filter coefficients highpass + + /* 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; + +//Specifying filter coefficients lowpass + + /* lowpass filter consists of three cascaded biquads + below the coefficients for those three biquads */ + //first high pass biquad + const double lowp1_a1 = -1.05207469728; + const double lowp1_a2 = 0.28586907478; + const double lowp1_b0 = 1.00000000000; + const double lowp1_b1 = 2.00000000000; + const double lowp1_b2 = 1.00000000000; + + //second high pass biquad + const double lowp2_a1 = -1.16338171052; + const double lowp2_a2 = 0.42191097989; + const double lowp2_b0 = 1.00000000000; + const double lowp2_b1 = 2.00000000000; + const double lowp2_b2 = 1.00000000000; + + //third high pass biquad + const double lowp3_a1 = -1.42439823874; + const double lowp3_a2 = 0.74093118112; + const double lowp3_b0 = 1.00000000000; + const double lowp3_b1 = 2.00000000000; + const double lowp3_b2 = 1.00000000000; + +//input to each filter is output of the filter before(excl. first which uses input_sample) + /* NOT SURE IF PASSING V1 AND V2 IS CORRECT, + WILL IT UPDATE IN THE MEMORY POSITION SO THAT + V1 IS CHANGED GLOBALLY */ + +//highpass + +double highpass_filter(double input) +{ + double y1 = biquad(input, v1, v2, highp1_a1, highp1_a2, highp1_b0, highp1_b1, highp1_b2); + double y2 = biquad(y1, v1, v2, highp2_a1, highp2_a2, highp2_b0, highp2_b1, highp2_b2); + double y3 = biquad(y2, v1, v2, highp3_a1, highp3_a2, highp3_b0, highp3_b1, highp3_b2); + + return y3; +} + + +//rectifier +double rectify(double y3) +{ + y3 = fabs(y3); + return y3; +} + +//lowpass + +double lowpass_filter(double y3) +{ + double y4 = biquad(y3, v1, v2, lowp1_a1, lowp1_a2, lowp1_b0, lowp1_b1, lowp1_b2); + double y5 = biquad(y4, v1, v2, lowp2_a1, lowp2_a2, lowp2_b0, lowp2_b1, lowp2_b2); + double filtered_signal = biquad(y5, v1, v2, lowp3_a1, lowp3_a2, lowp3_b0, lowp3_b1, lowp3_b2); + + return filtered_signal; +} + +double filter(double input) +{ + /* function passes the input through the three filters + returns the final output value as filtered sample + this is used in check_state() function to determine state of system + */ + double y1 = highpass_filter(input); + double y2 = rectify(y3); + double filtered_signal = lowpass_filter(y3); + + return filtered_signal; +} \ No newline at end of file