Biorobotics
/
EMG_test
Script to plot both a raw EMG signal and the filtered signal in HIDscope
main.cpp
- Committer:
- roosbulthuis
- Date:
- 2015-10-26
- Revision:
- 1:f32f8eac8af1
- Parent:
- 0:c85b764527f4
- Child:
- 2:1d29b91bc46a
File content as of revision 1:f32f8eac8af1:
#include "mbed.h" //#include "read_filter_emg.h" //included for fabs() function #include <math.h> #include "HIDScope.h" Ticker sample_timer; HIDScope scope(1); AnalogIn analog_emg_left(A0); AnalogIn analog_emg_right(A1); double input_left = 0; double input_right = 0; double v1_left=0; double v2_left=0; double v1_right=0; double v2_right=0; double filter_left; double filter_right; //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 &v1, double &v2) { 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 &v1, double &v2) { 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, double &v1, double &v2) { /* 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, v1, v2); double y2 = rectify(y1); double filtered_signal = lowpass_filter(y2, v1, v2); return filtered_signal; } double test=1; void sample() { input_left = analog_emg_left.read(); input_right = analog_emg_right.read(); filter_left = filter(input_left, v1_left, v2_left); filter_right = filter(input_right, v1_right, v2_right); scope.set(1, input_left); scope.set(0, filter_left); scope.send(); } int main() { sample_timer.attach(&sample, 0.002); while(1){} }