EMG controlling calibration + filters biorobotics group 13 BMT
Dependencies: HIDScope biquadFilter mbed
Diff: main.cpp
- Revision:
- 0:2ded24549b70
- Child:
- 1:16165e207e70
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Oct 15 14:39:52 2015 +0000 @@ -0,0 +1,58 @@ +#include "mbed.h" +#include "HIDScope.h" + +// Standaard formule voor het biquad filter +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; + v2=v1; + v1=v; + return y; +} +// Filter1 = High pass filter tot 20 Hz + +double fh1_v1=0, fh1_v2=0, fh2_v1=0, fh2_v2=0; +const double fh1_a1=-0.84909054461, fh1_a2=0.00000000000, fh1_b0= 1, fh1_b1=-1, fh1_b2=0; +const double fh2_a1=-1.82553264091, fh2_a2=0.85001416809, fh2_b0= 1, fh2_b1=-2, fh2_b2=1; +Ticker FHIGH; +void Highpass_filter() +{ + biquad (u, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0, fh1_b1, fh1_b2); + biquad (u, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0, fh2_b1, fh2_b2); +} + +// Filter2 = Low pass filter na 60 Hz +double fl1_v1=0, fl1_v2=0, fl2_v1=0, fl2_v2=0; +const double fl1_a1=-0.66979455390, fl1_a2=0.00000000000, fl1_b0= 1, fl1_b1=1, fl1_b2=0; +const double fl2_a1=-1.55376616139, fl2_a2=0.68023470431, fl2_b0= 1, fl2_b1=2, fl2_b2=1; +Ticker FLOW; +void Lowpass_filter() +{ + biquad (u, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0, fl1_b1, fl1_b2); + biquad (u, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0, fl2_b1, fl2_b2); +} + +// Filter3 = Notch filter at 50 Hz +double fno1_v1=0, fno1_v2=0, fno2_v1=0, fno2_v2=0, fno3_v1=0, fno3_v2=0; +const double fno1_a1 = -1.87934916386, fno1_a2= 0.97731851355, fno1_b0= 1, fno1_b1= -1.90090686046, fno1_b2= 1; +const double fno2_a1 = -1.88341028603, fno2_a2= 0.98825147717, fno2_b0= 1, fno2_b1= -1.90090686046, fno2_b2= 1; +const double fno3_a1 = -1.89635403726, fno3_a2= 0.98894004849, fno3_b0= 1, fno3_b1= -1.90090686046, fno3_b2= 1; +Ticker FNOTCH; +void Notch_filter () +{ + biquad (u, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0, fno1_b1, fno1_b2); + biquad (u, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2); + biquad (u, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2); +} + +int main () +{ + FHIGH.attach_us(Highpass_filter, 1e4); + FLOW.attach_us(Lowpass_filter, 1e4); + FNOTCH.attach_us (Notch_filter, 1e4); +} + + +