Eindversie. LU: 07-11-13.
Dependencies: MODSERIAL mbed Encoder
Revision 27:5d0c94b991aa, committed 2013-11-01
- Comitter:
- Socrates
- Date:
- Fri Nov 01 09:16:46 2013 +0000
- Parent:
- 26:539f131cf07c
- Child:
- 28:2904487e0a1e
- Commit message:
- Voor begrenzing op vrijdag.
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Oct 31 09:51:47 2013 +0000 +++ b/main.cpp Fri Nov 01 09:16:46 2013 +0000 @@ -24,7 +24,8 @@ DigitalIn sluis2(PTE1); //Functies en flags. -void keep_in_range(float * in, float min, float max); +void keep_in_range(double * in, double min, double max); +void keep_in_rangeint(int * in, int min, int max); volatile bool looptimerflag; void setlooptimerflag(void) @@ -41,6 +42,8 @@ volatile bool calflag=false; volatile bool grensflag=false; +volatile bool frictionflag=true; + int main() { //Constantes en tickers. @@ -48,21 +51,21 @@ pwm_B.period(1.0/2500.0); Ticker looptimer; Timeout dirtimeout; - const float ts=0.004; + const double ts=0.004; looptimer.attach(setlooptimerflag,ts); - float numh1,numh2,denh2,numl1,numl2,numl3,denl2,denl3; - float xtr,ytr,y1tr,x1tr,ztr,z1tr,z2tr,yabstr,yabs1tr,yabs2tr,ktr; - float xbr,ybr,y1br,x1br,zbr,z1br,z2br,yabsbr,yabs1br,yabs2br,kbr; - float xtl,ytl,y1tl,x1tl,ztl,z1tl,z2tl,yabstl,yabs1tl,yabs2tl,ktl; - float xbl,ybl,y1bl,x1bl,zbl,z1bl,z2bl,yabsbl,yabs1bl,yabs2bl,kbl; - float zx,zy, xuit,yuit, rt; - float vxuit, vyuit, Ap, Ad, Ad1, kd, kp, ki, Ai, Ai1, Bp, Bd, Bd1, Bi, Bi1; - float for_A, for_B, ctrlA, ctrlB; - float Asluis, Bsluis, calA, calApos, calB, calBpos; - float gain, grens; + double numh1,numh2,denh2,numl1,numl2,numl3,denl2,denl3; + double xtr,ytr,y1tr,x1tr,ztr,z1tr,z2tr,yabstr,yabs1tr,yabs2tr,ktr; + double xbr,ybr,y1br,x1br,zbr,z1br,z2br,yabsbr,yabs1br,yabs2br,kbr; + double xtl,ytl,y1tl,x1tl,ztl,z1tl,z2tl,yabstl,yabs1tl,yabs2tl,ktl; + double xbl,ybl,y1bl,x1bl,zbl,z1bl,z2bl,yabsbl,yabs1bl,yabs2bl,kbl; + double zx,zy, xuit,yuit, rt; + double vxuit, vyuit, Ap, Ad, Ad1, kd, kp, ki, Ai, Ai1, Bp, Bd, Bd1, Bi, Bi1; + double for_A, for_B, ctrlA, ctrlB; + double Asluis, Bsluis, calA, calApos, calB, calBpos; + double gain, grens,friction; int calAdir, calBdir, xdir, ydir, Adir, Bdir; int ticka, tickb, refA,refB, errA, errB; - + int Aboven, Aonder, Bboven,Bonder; //Startwaarden. x1tr=0; y1tr=0; @@ -100,11 +103,16 @@ rt=0.032805; gain=5.0; grens=0.3; + friction=0.2; Ai1=0; Ad1=0; Bi1=0; Bd1=0; pc.baud(115200); + Aboven=500; + Aonder=-500; + Bboven=7000; + Bonder=-500; //Filtercoëfficienten. //High pass, 35Hz, 1e orde, 4 ms. @@ -127,48 +135,49 @@ //denl1=1; //denl2=-1.982228929792529; //denl3=0.982385450614126; - -//Tijdstap voor sinusinput -//float t; + +//Tijdstap voor sinusinput +//double t; //t=0; //Opzetje voor calibratie -while(calflag==true){ + while(calflag==true) { -Asluis=sluis1.read(); -Bsluis=sluis2.read(); + Asluis=sluis1.read(); + Bsluis=sluis2.read(); -while(Asluis>0.5) { -calA=0.3; -calAdir=1; -calApos=motor1.getPosition(); -while(abs(calApos<1500)){} //Ter voorkoming van schade. -Asluis=sluis1.read(); -motordirA.write(calAdir); -pwm_A.write(calA); -} -pwm_A.write(0); + while(Asluis>0.5) { + calA=0.3; + calAdir=1; + calApos=motor1.getPosition(); + while(abs(calApos<1500)) {} //Ter voorkoming van schade. + Asluis=sluis1.read(); + motordirA.write(calAdir); + pwm_A.write(calA); + } + pwm_A.write(0); -while(Bsluis>0.5) { -calB=0.3; -calBdir=1; -calBpos=motor2.getPosition(); -while(abs(calBpos>2000)){} -Bsluis=sluis2.read(); -motordirB.write(calBdir); -pwm_A.write(calB); -} -pwm_B.write(0); + while(Bsluis>0.5) { + calB=0.3; + calBdir=1; + calBpos=motor2.getPosition(); + while(abs(calBpos>2000)) {} + Bsluis=sluis2.read(); + motordirB.write(calBdir); + pwm_B.write(calB); + } + pwm_B.write(0); -motor1.setPosition(0); -motor2.setPosition(0); -calflag=false; -} + motor1.setPosition(0); + motor2.setPosition(0); + calflag=false; + } //Einde opzetje. //Loop. +wait(2); while(1) { - while(looptimerflag != true); + while(looptimerflag != true); { } looptimerflag = false; @@ -244,13 +253,13 @@ zy=zy*(1.0/(1.0-grens)); //Richting omdraaien met triceps. - if ((ztr>(zbr+0.1)) && dirflagx == true) { + if (ztr>0.1 && dirflagx == true) { dirflagx = false; xdir ^= 1; zx=0; dirtimeout.attach(tricheck,1.0); } - if ((ztl>(zbl+0.1)) && dirflagy == true) { + if (ztl>0.1 && dirflagy == true) { dirflagy = false; ydir ^= 1; zy=0; @@ -267,20 +276,65 @@ ticka=motor1.getPosition(); tickb=motor2.getPosition(); + + //Begrenzing. + if(grensflag==true) { + if(ticka>Aboven) { + //pwm_A.write(friction+0.4); + //motordirA.write(0); + zx=0; + zy=0; + //wait(0.1); + //motor1.setPosition(Aboven+50); + } + if(ticka<Aonder) { + //pwm_A.write(friction+0.4); + //motordirA.write(1); + zx=0; + zy=0; + //wait(0.01); + //motor1.setPosition(Aonder-50); +} + + if(tickb>Bboven) { + //pwm_B.write(friction+0.2); + //motordirB.write(0); + zx=0; + zy=0; + //wait(0.01); + //motor2.setPosition(Bboven+50); + } + if(tickb<Bonder) { + //pwm_B.write(friction+0.2); + //motordirB.write(1); + zx=0; + zy=0; + //wait(0.01); + //motor2.setPosition(Bonder-50); + } + } + vxuit=zx*4.0*pow(10.0,-1.0); // 4cm/s vyuit=zy*4.0*pow(10.0,-1.0); // 4cm/s xuit += ts*vxuit; yuit += ts*vyuit; +keep_in_range(&xuit, + refA=4123.0*atan2(yuit,xuit)/(2.0*PI); refB=4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt); + + + keep_in_rangeint(&refA,Aonder,Aboven); + keep_in_rangeint(&refB,Bonder,Bboven); + //refA=500*sin(t)*tanh(0.1*t); //refB=500*sin(t)*tanh(0.1*t); //t +=ts; errA=refA-ticka; errB=refB-tickb; - + //Controllers Ap=errA*kp; Ad=(errA-Ad1)*kd/ts; @@ -305,79 +359,51 @@ if(ctrlA<0) { Adir=0; - for_A=for_A-0.25; } else { Adir=1; } if(ctrlB<0) { Bdir=0; - for_B=for_B-0.25; } else { - Bdir=1; - for_B=for_B-0.25; + Bdir=1; } -if(0<for_A<0.25) -{ -for_A=for_A+0.25; -} -if(-0.25<for_A<-0.001); -{ -for_A=for_A-0.25; -} - -if(0<for_A<0.25) -{ -for_A=for_A+0.25; -} -if(-0.25<for_A<-0.001); -{ -for_A=for_A-0.25; -} keep_in_range(&for_A, -1,1); keep_in_range(&for_B, -1,1); - -//Begrenzing. -if(grensflag==true) -{ -if(ticka>1500){ -for_A=0.2; -Adir =1; -} -if(ticka<0){ -for_A=0.2; -Adir =0; -} -if(tickb>10000){ -for_B=0.2; -Bdir =1; -} -if(tickb<0){ -for_B=0.2; -Bdir =0; -} -} + + if (frictionflag==true) + { +for_A=abs(for_A)+friction+0.2; +for_B=abs(for_B)+friction; + +keep_in_range(&for_A, 0,1); +keep_in_range(&for_B, 0,1); + } motordirA.write(Adir); motordirB.write(Bdir); pwm_A.write(abs(for_A)); pwm_B.write(abs(for_B)); - if(pc.txBufferGetSize(0)-pc.txBufferGetCount() > 100){ + if(pc.txBufferGetSize(0)-pc.txBufferGetCount() > 100) { //pc.printf(" %f\n\r",zy); + pc.printf(" %f %f %f %f %i %i\n\r",zx,zy, for_A,for_B,refA,refB); + //pc.printf("A %i %i B %i %i\n\r",Adir, ticka,Bdir, tickb); //pc.printf(" %f %f %f %f \n\r",kbl,zy,vyuit,yuit); //pc.printf(" %i %i %i %f\n\r",tickb,refB,errB,for_B); //pc.printf(" %f %f %f %f \n\r",kbl,zy,vyuit, yuit); //pc.printf("A %i %i %i %i %f %f\n\r",ticka,refA,errA,ctrlA,for_A); - //pc.printf("B %i %i %i\n\r",tickb,refB,errB); - //pc.printf(" %f %f %f %f \n\r",kbl,zy,vyuit,yuit); + //pc.printf("tr %f br %f tl %f bl %f\n\r",ztr,zbr,ztl,zbl); //pc.printf("%i\n",motor2.getPosition()); - //pc.printf("%f\n\r",for_A); - pc.printf("%f\n\r",for_B); - } + //pc.printf("%f %f\n\r",for_A,for_B); + } } } -void keep_in_range(float * in, float min, float max) +void keep_in_range(double * in, double min, double max) +{ +*in > min ? *in < max? : *in = max: *in = min; +} +void keep_in_rangeint(int * in, int min, int max) { *in > min ? *in < max? : *in = max: *in = min; } \ No newline at end of file