Eindversie. LU: 07-11-13.
Dependencies: MODSERIAL mbed Encoder
Revision 20:013e9c00e058, committed 2013-10-29
- Comitter:
- Socrates
- Date:
- Tue Oct 29 11:11:19 2013 +0000
- Parent:
- 19:09c4b5249cec
- Child:
- 21:659f7c8ed328
- Commit message:
- 90 %;
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Oct 28 16:09:13 2013 +0000 +++ b/main.cpp Tue Oct 29 11:11:19 2013 +0000 @@ -2,26 +2,24 @@ #include "MODSERIAL.h" #include "encoder.h" #define PI 3.14159265358979323 - +//XenY //Een pwm van 0.05 is net genoeg om de heugel te bewegen. linksom bewegen is negatief voor de encoder. getposition gaat in ticks. //4123 ticks is een rondje. - -//XenY +//Rechts is x, links is y AnalogIn emgtr(PTB3); AnalogIn emgbr(PTB2); -//Rechts is x, links is y AnalogIn emgtl(PTB1); AnalogIn emgbl(PTB0); -PwmOut pwm_A(PTA12); //Motor A -PwmOut pwm_B(PTA5);//Motor B +PwmOut pwm_A(PTA12); +PwmOut pwm_B(PTA5); MODSERIAL pc(USBTX,USBRX); DigitalOut motordirA(PTD3); DigitalOut motordirB(PTD1); Encoder motor1(PTD0,PTC9); Encoder motor2(PTD5,PTC8); + void keep_in_range(float * in, float min, float max); - volatile bool looptimerflag; void setlooptimerflag(void) { @@ -39,8 +37,8 @@ int main() { -pwm_A.period(1.0/22000.0); -pwm_B.period(1.0/22000.0); + pwm_A.period(1.0/22000.0); + pwm_B.period(1.0/22000.0); Ticker looptimer; Timeout dirtimeout; const float ts=0.001; @@ -51,31 +49,30 @@ 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; - float gain, xuit, kp, ki, zx1, yuit, zy1, pwmA, pwmB,rt; - int xdir; - int ydir; + float xuit,yuit, rt; + //float zx1, zy1, gain; + //float pwmA, pwmB; + int xdir, ydir, Adir, Bdir; float vxuit, vyuit, Ap, Ad, Ad1, kd, kp, ki, Ai, Ai1, Bp, Bd, Bd1, Bi, Bi1; - float for_A, for_B; - int ticka, tickb, refA,refB, errA, errB, - + float for_A, for_B, yuit1, xuit1; + int ticka, tickb, refA,refB, errA, errB; + float ctrlA; + float ctrlB; + //Startwaarden x1tr=0; y1tr=0; z1tr=0; z2tr=0; yabs1tr=0; yabs2tr=0; x1br=0; y1br=0; z1br=0; z2br=0; yabs1br=0; yabs2br=0; x1tl=0; y1tl=0; z1tl=0; z2tl=0; yabs1tl=0; yabs2tl=0; x1bl=0; y1bl=0; z1bl=0; z2bl=0; yabs1bl=0; yabs2bl=0; - zx=0; zy=0; xdir=0; ydir=0; - kp=0; ki=1; zx1=0; zy1=0; rt=32.805/1000.0; + zx=0; zy=0; xdir=0; ydir=0; xuit=0; yuit=0; + kp=0.01; ki=0.01; kd=0.01; rt=0.032805; + Bi1=0; Bd1=0; yuit1=0; xuit1=0; - - //High pass, 35Hz, 1e + //High pass, 35Hz, 1e orde numh1=0.900575535279376; numh2=-0.900575535279376; //denh1=1; denh2=-0.801151070558751; - - //Low pass, 2 Hz, 2e orde - //numl1=0.391302053991682*pow(10.0,-4.0); - //numl2=0.782604107983365*pow(10.0,-4.0); - //numl3=0.391302053991682*pow(10.0,-4.0); + //Low pass, 5 Hz, 2e orde. numl1=0.241359049041961*pow(10.0,-3.0); numl2=0.482718098083923*pow(10.0,-3.0); @@ -119,8 +116,8 @@ zbl=yabsbl*numl1+yabs1bl*numl2+yabs2bl*numl3-z1bl*denl2-z2bl*denl3; x1bl=xbl; y1bl=ybl; z2bl=z1bl; z1bl=zbl; yabs2bl=yabs1bl; yabs1bl=yabsbl; - zx=(zbr); - zy=(zbl); + zx=(zbr*5.0); + zy=(zbl*5.0); //Grenzen. if (zx>1.0) { @@ -129,10 +126,10 @@ if (zy>1.0) { zy=0.99999; } - if (zx<0.30){ + if (zx<0.20){ zx=0; } - if (zy<0.30){ + if (zy<0.20){ zy=0; } @@ -140,11 +137,13 @@ if ((ztr>(zbr+0.1)) && dirflagx == true) { dirflagx = false; xdir ^= 1; + zx=0; dirtimeout.attach(tricheck,1.5); } if ((ztl>(zbl+0.1)) && dirflagy == true) { dirflagy = false; ydir ^= 1; + zy=0; dirtimeout.attach(tricheck,1.5); } @@ -159,35 +158,49 @@ } ticka=motor1.getPosition(); tickb=motor2.getPosition(); - vxuit=zx*4.0*pow(10.0,-5.0); // 4cm/s - vyuit=zy*4.0*pow(10.0,-5.0); // 4cm/s + vxuit=zx*1.0*pow(1.0,-2.0); // 4cm/s + vyuit=zy*1.0*pow(1.0,-2.0); // 4cm/s xuit += ts*vxuit; - yuiy += ts*vyuit; + yuit += ts*vyuit; + yuit1=yuit; + + refA=floor(4123.0*atan2(yuit,xuit)/(2.0*PI)); + refB=floor(4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt)); + errA=refA-ticka; + errB=refB-tickb; - refA=4123.0*atan2(yuit,xuit)/(2.0*PI); - refB=4123.0*sqrt(xuit*xuit+yuit*yuit)*(2.0*PI*rt);//nog keer omtrek - errA=refA-ticka; - errB=refB-tickb; - +if(errA<0){ +Adir=0;} +else{ +Adir=1; +} +if(errB<0){ +Bdir=0;} +else{ +Bdir=1; +} //Controllers Ap=errA*kp; Ad=(errA-Ad1)*kd/ts; Ai=(Ai1+ts*errA)*ki; Ad1=Ad; Ai1=Ai; - for_a=Ai+Ap+Ad; + ctrlA=float(Ai+Ap+Ad); + for_A=(ctrlA)/1000.0; + Bp=errB*kp; Bd=(errB-Bd1)*kd/ts; Bi=(Bi1+ts*errB)*ki; Bd1=Bd; Bi1=Bi; - for_B=Bi+Bp+Bd; + ctrlB=float(Bi+Bp+Bd); + for_B=(ctrlB)/1000.0; //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks. keep_in_range(&for_A, -1,1); keep_in_range(&for_B, -1,1); - motordirA.write(xdir); - motordirB.write(ydir); + motordirA.write(Adir); + motordirB.write(Bdir); pwm_A.write(for_A); pwm_B.write(for_B); //pc.printf("Bi: %f, Tri: %f, Richting: %d \n\r",zbl*3.0,ztl*3.0,ydir); - pc.printf(" %f\n\r",zy); + pc.printf(" %i %i %i %f %f\n\r",tickb,refB,errB,ctrlB,for_B); //pc.printf("b %f t %f xu %f yu %f A %f B %f \n\r",zbl,ztl,xuit, yuit, pwmA,pwmB); } }