Richell Booyink / Mbed 2 deprecated Filter

Dit is het filter dat werkt. Bij het werkende filter was de rectifier nog niet toegevoegd. Deze heb ik nu wel toegevoegd, maar hij staat nu nog tussen haken, zodat je eerst zonder en dan met kunt proberen. Misschien dat het daaraan ligt

Dependencies:   HIDScope mbed

main.cpp@1:dd4d1be5a7ab, 2015-10-21 (annotated)

Committer:
RichellBooyink
Date:
Wed Oct 21 17:26:56 2015 +0000
Revision:
1:dd4d1be5a7ab
Parent:
0:96915439a15f 
Dit is het filter dat werkt. Hierbij is de rectifier toegevoegd, deze staat nog tussen haken;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RichellBooyink 0:96915439a15f 1 #include "mbed.h"
RichellBooyink 0:96915439a15f 2 #include "HIDScope.h"
RichellBooyink 0:96915439a15f 3
RichellBooyink 0:96915439a15f 4 AnalogIn EMG(A0);
RichellBooyink 0:96915439a15f 5 HIDScope scope(4);
RichellBooyink 0:96915439a15f 6
RichellBooyink 0:96915439a15f 7 // Filter1 = High pass filter tot 20 Hz
RichellBooyink 0:96915439a15f 8 double fh1_v1=0, fh1_v2=0, fh2_v1=0, fh2_v2=0;
RichellBooyink 0:96915439a15f 9 const double fh1_a1=-0.84909054461, fh1_a2=0.00000000000, fh1_b0= 1, fh1_b1=-1, fh1_b2=0;
RichellBooyink 0:96915439a15f 10 const double fh2_a1=-1.82553264091, fh2_a2=0.85001416809, fh2_b0= 1, fh2_b1=-2, fh2_b2=1;
RichellBooyink 0:96915439a15f 11 // Filter2 = Low pass filter na 60 Hz
RichellBooyink 0:96915439a15f 12 double fl1_v1=0, fl1_v2=0, fl2_v1=0, fl2_v2=0;
RichellBooyink 0:96915439a15f 13 const double fl1_a1=-0.66979455390, fl1_a2=0.00000000000, fl1_b0= 1, fl1_b1=1, fl1_b2=0;
RichellBooyink 0:96915439a15f 14 const double fl2_a1=-1.55376616139, fl2_a2=0.68023470431, fl2_b0= 1, fl2_b1=2, fl2_b2=1;
RichellBooyink 0:96915439a15f 15 // Filter3 = Notch filter at 50 Hz
RichellBooyink 0:96915439a15f 16 double fno1_v1=0, fno1_v2=0, fno2_v1=0, fno2_v2=0, fno3_v1=0, fno3_v2=0;
RichellBooyink 0:96915439a15f 17 const double fno1_a1 = -1.87934916386, fno1_a2= 0.97731851355, fno1_b0= 1, fno1_b1= -1.90090686046, fno1_b2= 1;
RichellBooyink 0:96915439a15f 18 const double fno2_a1 = -1.88341028603, fno2_a2= 0.98825147717, fno2_b0= 1, fno2_b1= -1.90090686046, fno2_b2= 1;
RichellBooyink 0:96915439a15f 19 const double fno3_a1 = -1.89635403726, fno3_a2= 0.98894004849, fno3_b0= 1, fno3_b1= -1.90090686046, fno3_b2= 1;
RichellBooyink 0:96915439a15f 20 double y1, y2, y3, y4, y5, y6, y7, u1, u2, u3, u4, u5, u6, u7;
RichellBooyink 0:96915439a15f 21
RichellBooyink 0:96915439a15f 22 // Standaard formule voor het biquad filter
RichellBooyink 0:96915439a15f 23 double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
RichellBooyink 0:96915439a15f 24
RichellBooyink 0:96915439a15f 25 {
RichellBooyink 0:96915439a15f 26 double v = u - a1*v1 - a2*v2;
RichellBooyink 0:96915439a15f 27 double y = b0*v + b1*v1 + b2*v2;
RichellBooyink 0:96915439a15f 28 v2=v1;
RichellBooyink 0:96915439a15f 29 v1=v;
RichellBooyink 0:96915439a15f 30 return y;
RichellBooyink 0:96915439a15f 31 }
RichellBooyink 0:96915439a15f 32
RichellBooyink 0:96915439a15f 33 void Filters()
RichellBooyink 0:96915439a15f 34 {
RichellBooyink 0:96915439a15f 35 u1 = EMG.read();
RichellBooyink 0:96915439a15f 36 //Highpass
RichellBooyink 0:96915439a15f 37 y1 = biquad (u1, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547);
RichellBooyink 0:96915439a15f 38 u2 = y1;
RichellBooyink 0:96915439a15f 39 y2 = biquad (u2, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885);
RichellBooyink 0:96915439a15f 40 //Lowpass
RichellBooyink 0:96915439a15f 41 u3 = y2;
RichellBooyink 0:96915439a15f 42 y3 = biquad (u3, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103);
RichellBooyink 0:96915439a15f 43 u4 = y3;
RichellBooyink 0:96915439a15f 44 y4 = biquad (u4, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617);
RichellBooyink 0:96915439a15f 45 // Notch
RichellBooyink 0:96915439a15f 46 u5 = y4;
RichellBooyink 0:96915439a15f 47 y5 = biquad (u5, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206);
RichellBooyink 0:96915439a15f 48 u6 = y5;
RichellBooyink 0:96915439a15f 49 y6 = biquad (u6, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2);
RichellBooyink 0:96915439a15f 50 u7 = y6;
RichellBooyink 0:96915439a15f 51 y7 = biquad (u7, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227);
RichellBooyink 1:dd4d1be5a7ab 52 //y8 = fabs (y7)
RichellBooyink 0:96915439a15f 53 scope.set (0, u1);
RichellBooyink 0:96915439a15f 54 scope.set (1, y2);
RichellBooyink 0:96915439a15f 55 scope.set (2, y4);
RichellBooyink 0:96915439a15f 56 scope.set (3, y7);
RichellBooyink 0:96915439a15f 57 scope.send ();
RichellBooyink 0:96915439a15f 58 }
RichellBooyink 0:96915439a15f 59 Ticker filter;
RichellBooyink 0:96915439a15f 60 int main ()
RichellBooyink 0:96915439a15f 61 {filter.attach_us(Filters,1e3);}
RichellBooyink 0:96915439a15f 62