Biorobotics
/
piano_robot
newest version,
Diff: read_filter_emg.cpp
- Revision:
- 0:fc6fa085d591
- Child:
- 1:5c1fc4e9401a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/read_filter_emg.cpp Fri Oct 23 13:14:57 2015 +0000 @@ -0,0 +1,149 @@ +/* + * read_filter_emg.cpp + * + * Created on: Oct 14, 2015 + * Author: User + */ + +#include "mbed.h" +#include "read_filter_emg.h" +//included for fabs() function +#include <math.h> +//EMG::EMG(AnalogIn aI_, double &v1_, double &v2_) + +//EMG constructor +EMG::EMG(PinName aI_, double &v1_, double &v2_) +{ + //initialise the variables of the EMG instance + // ?????? NOT SURE HOW THIS SECTION SHOULD WORK EXACTLY, WHAT SHOULD I DEFINE, WHAT SHOULD I 'RENAME' + // E.G aI = aI_, v1 = v1_... + // + AnalogIn aI(aI_); + /* two past values of v1 and v2 initialised in main are + passed to the function as they need to be kept track of + cannot be reset each time as filtering would be incorrect + */ + v1 = v1_; + v2 = v2_; + //initialised to zero, updated in sample() + input_sample = 0; + //initialised to zero, calculated in filter function using input_sample + filtered_sample = 0; +}; + +void EMG::sample() +{ + //change value of sample to input of the EMG + //should i use the reference? + input_sample = aI.read(); +} + +//general biquad filter that can be called in all the filter functions +double EMG::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; +} + +double EMG::highpass_filter(double input) +{ + /* 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; + + //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 */ + double y1 = EMG::biquad(input, v1, v2, highp1_a1, highp1_a2, highp1_b0, highp1_b1, highp1_b2); + double y2 = EMG::biquad(y1, v1, v2, highp2_a1, highp2_a2, highp2_b0, highp2_b1, highp2_b2); + double y3 = EMG::biquad(y2, v1, v2, highp3_a1, highp3_a2, highp3_b0, highp3_b1, highp3_b2); + + return y3; +} + +//need to implement +double rectify(double input) +{ + input = fabs(input); + return input; +} +double EMG::lowpass_filter(double input) +{ + /* 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 */ + double y1 = EMG::biquad(input, v1, v2, lowp1_a1, lowp1_a2, lowp1_b0, lowp1_b1, lowp1_b2); + double y2 = EMG::biquad(y1, v1, v2, lowp2_a1, lowp2_a2, lowp2_b0, lowp2_b1, lowp2_b2); + double y3 = EMG::biquad(y2, v1, v2, lowp3_a1, lowp3_a2, lowp3_b0, lowp3_b1, lowp3_b2); + + return y3; +} + +double EMG::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(y1); + double y3 = lowpass_filter(y3); + filtered_sample = y3; + + return filtered_sample; +} + + + + + +