![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Script to plot both a raw EMG signal and the filtered signal in HIDscope
main.cpp
- Committer:
- yohoo15
- Date:
- 2015-10-27
- Revision:
- 5:cf85007ce40a
- Parent:
- 4:d32946ab9c54
File content as of revision 5:cf85007ce40a:
#include "mbed.h" //#include "read_filter_emg.h" //included for fabs() function #include <math.h> #include "HIDScope.h" #include <iostream> Ticker HIDScope_timer; Ticker Filteren_timer; HIDScope scope(2); // defining flags volatile bool Flag_filteren = false; volatile bool Flag_HIDScope = false; // making function flags. void Go_flag_filteren() { Flag_filteren = true; } void Go_flag_HIDScope() { Flag_HIDScope = true; } AnalogIn analog_emg_left(A0); //AnalogIn analog_emg_right(A1); double input = 0; double filter_signal; //double input_right = 0; double v1=0; double v2=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 /* 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; //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 */ //notch double notch_filter(double input, double v1, double v2) { double y1 = biquad(input, v1, v2, notch1_a1, notch1_a2, notch1_b0, notch1_b1, notch1_b2); double y2 = biquad(y1, v1, v2, notch2_a1, notch2_a2, notch2_b0, notch2_b1, notch2_b2); double y3 = biquad(y2, v1, v2, notch3_a1, notch3_a2, notch3_b0, notch3_b1, notch3_b2); return y3; } //highpass double highpass_filter(double y3, double v1, double v2) { double y4 = biquad(y3, v1, v2, highp1_a1, highp1_a2, highp1_b0, highp1_b1, highp1_b2); double y5 = biquad(y4, v1, v2, highp2_a1, highp2_a2, highp2_b0, highp2_b1, highp2_b2); double y6 = biquad(y5, v1, v2, highp3_a1, highp3_a2, highp3_b0, highp3_b1, highp3_b2); return y6; } //rectifier double rectify(double y6) { y6 = fabs(y6); return y6; } //lowpass double lowpass_filter(double y6, double v1, double v2) { double y7 = biquad(y6, v1, v2, lowp1_a1, lowp1_a2, lowp1_b0, lowp1_b1, lowp1_b2); double y8 = biquad(y7, v1, v2, lowp2_a1, lowp2_a2, lowp2_b0, lowp2_b1, lowp2_b2); double filtered_signal = biquad(y8, 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 y0 = notch_filter(input, v1, v2); double y1 = highpass_filter(y0, v1, v2); double y2 = rectify(y1); double filtered_signal = lowpass_filter(y2, v1, v2); return filtered_signal; } double test=1; void Filteren() { input = analog_emg_left.read(); input = input-0.45; //FIRST SUBTRACT MEAN THEN FILTER //input_right = analog_emg_right.read(); filter_signal = filter(input, v1, v2); //filter_right = filter(input_right, v1_right, v2_right); } void HIDScope_kijken() { scope.set(0, input); scope.set(1, filter_signal); scope.send(); } int main() { HIDScope_timer.attach(&Go_flag_HIDScope, 0.002); Filteren_timer.attach(&Go_flag_filteren,0.004); while(1){ if(Flag_filteren) { Flag_filteren = false; Filteren(); } if(Flag_HIDScope) { Flag_HIDScope = false; HIDScope_kijken(); } } }