Eindversie. LU: 07-11-13.
Dependencies: MODSERIAL mbed Encoder
Revision 4:513c33bba011, committed 2013-10-16
- Comitter:
- Socrates
- Date:
- Wed Oct 16 15:00:40 2013 +0000
- Parent:
- 3:935347ab4e12
- Child:
- 5:1a1ce2f5cb66
- Commit message:
- Werkende filters. High 1e op 1 hz, low 2e op 2 hz.
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Oct 16 08:54:31 2013 +0000 +++ b/main.cpp Wed Oct 16 15:00:40 2013 +0000 @@ -17,20 +17,25 @@ Ticker looptimer; const float ts=0.001; looptimer.attach(setlooptimerflag,ts); - float x,y,y1,x1,z1,yabs,z,yabs1,k,numh1,numh2,denh1,denh2,numl1,numl2,denl1,denl2; + float x,y,y1,y2,x1,z,z1,z2,yabs,yabs1,yabs2,k,numh1,numh2,denh1,denh2,numl1,numl2,numl3,denl1,denl2,denl3; x1=0; y1=0; + y2=0; z1=0; + z2=0; yabs1=0; + yabs2=0; numh1=0.996868235770807; numh2=-0.996868235770807; //denh1=1; denh2=-0.993736471541615; - numl1=0.006244035046343; - numl2=-0.006244035046343; + numl1=0.391302053991682*pow(10.0,-4.0); + numl2=0.782604107983365*pow(10.0,-4.0); + numl3=0.391302053991682*pow(10.0,-4.0); //denl1=1; - denl2=-0.987511929907314; + denl2=-1.982228929792529; + denl3=0.982385450614126; pc.baud(115200); while(1) { @@ -40,27 +45,23 @@ x=(k-0.5)*2.0; //High pass, 1 Hz - //Waarom heb ik geen - gebruikt bij y1/denh2? - //y=(y1/1.000994230391224-(0.996868235770807/1.000994230391224)*x1+(0.997865599482850/1.000994230391224)*x); - //Met bovenstaande zijn de metingen gedaan. - //Dat is - //y=(y1/denh2-(numh1/denh2)*x1+(numh2/denh2)*x) + //1e orde y=x*numh1+x1*numh2-y1*denh2; + //Rectify yabs=abs(y); - //Low pass, 1 Hz - //Waarom heb ik geen - gebruikt bij z1/denl2? - //z=(z1/1.000987999680353-(0.006244035046343/1.000987999680353)*yabs1+(0.006237787927252/1.000987999680353)*yabs); - //Met bovenstaande zijn de metingen gedaan. - //Dat is - //z=(z1/denl2-(numl1/denl2)*y1+(numl2/denl2)*y) - z=yabs*numl1+yabs1*numl2-z1*denl2; + //Low pass, 2 Hz + //2e orde + z=yabs*numl1+yabs1*numl2+yabs2*numl3-z1*denl2-z2*denl3; pc.printf("%f\n\r",z); x1=x; + y2=y1; y1=y; + z2=z1; z1=z; + yabs2=yabs; yabs1=yabs; } } \ No newline at end of file