![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
For BIOROBOTICS PROJECT
Dependencies: HIDScope MODSERIAL QEI mbed
Revision 2:ac98d055a6cd, committed 2015-10-28
- Comitter:
- dbayuadi
- Date:
- Wed Oct 28 09:47:23 2015 +0000
- Parent:
- 1:3bae5ab25e20
- Commit message:
- filter EMG
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 3bae5ab25e20 -r ac98d055a6cd main.cpp --- a/main.cpp Wed Oct 21 12:31:55 2015 +0000 +++ b/main.cpp Wed Oct 28 09:47:23 2015 +0000 @@ -1,55 +1,58 @@ #include "mbed.h" #include "HIDScope.h" #include "QEI.h" -AnalogIn emg(A0); -HIDScope scope(5); +AnalogIn emg_left(A0); +AnalogIn emg_right(A1); +DigitalOut led(LED_BLUE); +HIDScope scope(6); Ticker get; AnalogIn pot(A1); Serial pc(USBTX, USBRX); // tx, rx -const int N = 10; volatile bool fn_go = false; double y_notch_1; double y_notch_2; double y_high; double y_low; double y_rect; -const double Fs = 500; -const double Ts = 0.002; +const double Fs = 500*2; +const double Ts = 1/Fs; double a; -const double gain_notch_1 = 0.912483; +const double gain_notch_1 = 1.000000; const double b0_notch_1 = 1.0*gain_notch_1; -const double b1_notch_1 = -1.62829098849*gain_notch_1; +const double b1_notch_1 = -1.90215058158*gain_notch_1; const double b2_notch_1 = 1.0*gain_notch_1; -const double a1_notch_1 = -1.49965530713; -const double a2_notch_1 = 0.90720693582; +const double a1_notch_1 = -1.76851639902; +const double a2_notch_1 = 0.89458928499; double v1_notch_1 = 0, v2_notch_1 = 0; const double gain_notch_2 = 1.000000; const double b0_notch_2 = 1.0*gain_notch_2; -const double b1_notch_2 = -1.63220522905*gain_notch_2; +const double b1_notch_2 = -1.90215058158*gain_notch_2; const double b2_notch_2 = 1.0*gain_notch_2; -const double a1_notch_2 = -1.61200955424; -const double a2_notch_2 = 0.91813588764; +const double a1_notch_2 = -1.85196790295; +const double a2_notch_2 = 0.92089778695; double v1_notch_2 = 0, v2_notch_2 = 0; +//high pass of 5 Hz Wc -const double gain_high = 0.981587; -const double b0_high = 1.0*gain_high; -const double b1_high = -1.99999992457*gain_high; -const double b2_high = 1.0*gain_high; -const double a1_high = -1.96306054568; -const double a2_high = 0.96374056522; +const double gain_high = 1; +const double b0_high = 0.978030479206560*gain_high; +const double b1_high = -1.956060958413119*gain_high; +const double b2_high = 0.978030479206560*gain_high; +const double a1_high = -1.955578240315036; +const double a2_high = 0.956543676511203; double v1_high = 0, v2_high = 0; +double v1_high_1 = 0, v2_high_1=0; -const double gain_low = 0.000561; -const double b0_low = 1.0*gain_low; -const double b1_low = 2.000*gain_low; -const double b2_low = 1.0*gain_low; -const double a1_low = -1.93191036285; -const double a2_low = 0.93415335760; +//low pass of 0.3 Hz Wc +const double gain_low = 0.00001; +const double b0_low = 0.088708177364838*gain_low; +const double b1_low = 0.177416354729676*gain_low; +const double b2_low = 0.088708177364838*gain_low; +const double a1_low = -1.997334271812535; +const double a2_low = 0.997337820139629; double v1_low = 0, v2_low = 0; - - +double v1_low_1 = 0,v2_low_1=0; 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; @@ -59,58 +62,135 @@ return y; } +double biquad1(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; + v2 = v1; + v1 = v; + return y; +} + +double biquad2(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; + v2 = v1; + v1 = v; + return y; +} + +double biquad3(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; + v2 = v1; + v1 = v; + return y; +} + +double biquad4(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; + v2 = v1; + v1 = v; + return y; +} + + + + void fn_activate() { fn_go = true; } -double sliding_average(double u,const int f_N) +double z1; +double y_high_1; +double y_notch_1_1; +double y_notch_2_1; +double y_rect_1; +double y_low_1; +double t = 0; +double value = 0; +double max() // calibration { - double f_x[f_N]; - double f_sum=0; - f_x[1]=abs(u); - for (int i=f_N; i>=1; i--){ - f_x[i]=f_x[i-1]; + if (t <= 10) { + value = value + 0.0; + } else if(t>10 && t<=20) { + if(value<=y_low) { + value = y_low; + + } else { + value = value +0.0; } - - - for (int i=f_N; i>=0; i--){ - f_sum=f_x[i]+f_sum; - } - a=f_sum/double(f_N); - f_sum=0; - return a; + + } else { + value = value + 0.0; + + + } + return value*0.80; } -double z1; +double val; +double gain() +{ + + if(max()==0) { + val = 0; + } else if(max()>=0) { + val = 1/max(); + } + return val; +} +const double a1 = 11.1111; +const double b1 = -0.1111; +const double a2 = 4.7619; +const double b2 = -0.047619; void scopeSend() { - y_high = biquad(emg.read(), v1_high, v1_high, a1_high, a2_high, b0_high, b1_high, b2_high); - y_notch_1 = biquad(y_high, v1_notch_1, v1_notch_1, a1_notch_1,a2_notch_1,b0_notch_1, b1_notch_1, b2_notch_1); - y_notch_2 = biquad(y_notch_1, v1_notch_2, v1_notch_2, a1_notch_2, a2_notch_2, b0_notch_2, b1_notch_2, b2_notch_2); - y_rect = fabs(y_notch_2); - y_low = biquad(y_rect, v1_low, v1_low, a1_low, a2_low, b0_low, b1_low, b2_low); -// z1 = 6*sliding_average(emg.read(), N); //moving average -scope.set(0,emg.read()); -scope.set(1,y_high); - scope.set(2,y_rect); - scope.set(3,y_low); - // scope.set(4,z1); + y_high = biquad1(emg_left.read(), v1_high, v2_high, a1_high, a2_high, b0_high, b1_high, b2_high); + y_rect = abs(y_high); + y_low = biquad2(y_rect, v1_low, v2_low, a1_low, a2_low, b0_low, b1_low, b2_low); + t = t+Ts; + y_high_1 = biquad3(emg_right.read(), v1_high_1, v2_high_1, a1_high, a2_high, b0_high, b1_high, b2_high); + y_rect_1 = abs(y_high_1); + y_low_1 = biquad4(y_rect_1, v1_low_1, v2_low_1, a1_low, a2_low, b0_low, b1_low, b2_low); + + scope.set(0,emg_left.read()); + scope.set(1,emg_right.read()); + scope.set(2,y_low); + scope.set(3,y_low_1); + scope.set(4,a1*y_low + b1); + scope.set(5,a2*y_low_1 + b2); + scope.send(); } int main() { -get.attach(&fn_activate,Ts); + get.attach(&fn_activate,Ts); while (true) { - + if(t<=10) { + led = 0; + } + if(t>10 && t<=20) + { + led = 1; + } + if(t>20) + { + led = 0; + } if(fn_go == true) { - + scopeSend(); - - + + fn_go = false; } }