Eindversie. LU: 07-11-13.
Dependencies: MODSERIAL mbed Encoder
Revision 26:539f131cf07c, committed 2013-10-31
- Comitter:
- Socrates
- Date:
- Thu Oct 31 09:51:47 2013 +0000
- Parent:
- 25:bfe7c49e76cd
- Child:
- 27:5d0c94b991aa
- Commit message:
- voor gerard en lichtpoorten
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Oct 30 16:06:26 2013 +0000 +++ b/main.cpp Thu Oct 31 09:51:47 2013 +0000 @@ -6,6 +6,8 @@ //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. //Rechts is x, links is y + +//Inputs. AnalogIn emgtr(PTB3); AnalogIn emgbr(PTB2); AnalogIn emgtl(PTB1); @@ -18,6 +20,10 @@ Encoder motor1(PTD0,PTC9); Encoder motor2(PTD5,PTC8); +DigitalIn sluis1(PTE0); +DigitalIn sluis2(PTE1); + +//Functies en flags. void keep_in_range(float * in, float min, float max); volatile bool looptimerflag; @@ -25,20 +31,21 @@ { looptimerflag = true; } - volatile bool dirflagx=true; volatile bool dirflagy=true; - void tricheck(void) { dirflagx=true; dirflagy=true; } +volatile bool calflag=false; +volatile bool grensflag=false; int main() { - pwm_A.period(1.0/1000.0); - pwm_B.period(1.0/1000.0); +//Constantes en tickers. + pwm_A.period(1.0/2500.0); + pwm_B.period(1.0/2500.0); Ticker looptimer; Timeout dirtimeout; const float ts=0.004; @@ -48,15 +55,15 @@ 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; - float xuit,yuit, rt; - int xdir, ydir, Adir, Bdir; + 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; + float for_A, for_B, ctrlA, ctrlB; + float Asluis, Bsluis, calA, calApos, calB, calBpos; + float gain, grens; + int calAdir, calBdir, xdir, ydir, Adir, Bdir; int ticka, tickb, refA,refB, errA, errB; - float ctrlA; - float ctrlB; - //Startwaarden + +//Startwaarden. x1tr=0; y1tr=0; z1tr=0; @@ -89,38 +96,79 @@ yuit=0; kp=1.0*0.1545; kd=1.0*0.0*2.8*pow(10.0,-3.0); - ki=0.0*1.0; + ki=0.1*1.0; rt=0.032805; + gain=5.0; + grens=0.3; Ai1=0; Ad1=0; Bi1=0; Bd1=0; + pc.baud(115200); - //High pass, 35Hz, 1e orde 4 ms +//Filtercoëfficienten. + //High pass, 35Hz, 1e orde, 4 ms. numh1=0.680011076547878; numh2=-0.680011076547878; //denh1=1; denh2=-0.360022153095757; - //Low pass, 5 Hz, 2e orde. 4 ms + //Low pass, 5 Hz, 2e orde, 4 ms. numl1=0.003621681514929; numl2=0.007243363029857; numl3=0.003621681514929; //denl1=1; denl2=-1.822694925196308; denl3=0.837181651256023; - //Low pass, 2 Hz, 2e orde. 1 ms + //Low pass, 2 Hz, 2e orde, 1 ms. //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=-1.982228929792529; //denl3=0.982385450614126; -float t; -t=0; - pc.baud(115200); + +//Tijdstap voor sinusinput +//float t; +//t=0; + +//Opzetje voor calibratie +while(calflag==true){ + +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(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); + +motor1.setPosition(0); +motor2.setPosition(0); +calflag=false; +} +//Einde opzetje. + +//Loop. while(1) { - while(looptimerflag != true); + while(looptimerflag != true); { } looptimerflag = false; @@ -173,27 +221,29 @@ yabs2bl=yabs1bl; yabs1bl=yabsbl; - zx=(zbr*5.0); - zy=(zbl*5.0); +//Gains om filter te compenseren. + zx=(zbr*gain); + zy=(zbl*gain); - //Grenzen. +//Grenzen. if (zx>1.0) { zx=0.99999; } if (zy>1.0) { zy=0.99999; } - if (zx<0.3) { - zx=0.3; + if (zx<grens) { + zx=grens; } - if (zy<0.3) { - zy=0.3; + if (zy<grens) { + zy=grens; } - zx=zx-0.3; - zx=zx*(1.0/0.7); - zy=zy-0.3; - zy=zy*(1.0/0.7); - //Richting omdraaien met triceps. + zx=zx-grens; + zx=zx*(1.0/(1.0-grens)); + zy=zy-grens; + zy=zy*(1.0/(1.0-grens)); + +//Richting omdraaien met triceps. if ((ztr>(zbr+0.1)) && dirflagx == true) { dirflagx = false; xdir ^= 1; @@ -222,11 +272,11 @@ xuit += ts*vxuit; yuit += ts*vyuit; - refA=(4123.0*atan2(yuit,xuit)/(2.0*PI)); - refB=(4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt)); - refA=500*sin(t)*tanh(0.1*t); - refB=500*sin(t)*tanh(0.1*t); - t +=ts; + refA=4123.0*atan2(yuit,xuit)/(2.0*PI); + refB=4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt); + //refA=500*sin(t)*tanh(0.1*t); + //refB=500*sin(t)*tanh(0.1*t); + //t +=ts; errA=refA-ticka; errB=refB-tickb; @@ -240,7 +290,7 @@ Ad1=Ad; Ai1=Ai; ctrlA=(Ai+Ap+Ad); - for_A=(ctrlA)/10.0; + for_A=(ctrlA)/1000.0; Bp=errB*kp; Bd=(errB-Bd1)*kd/ts; @@ -250,35 +300,79 @@ Bd1=Bd; Bi1=Bi; ctrlB=(Bi+Bp+Bd); - for_B=(ctrlB)/10.0; + for_B=(ctrlB)/1000.0; //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks. 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; + Bdir=1; + for_B=for_B-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; +} + +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; +} +} motordirA.write(Adir); motordirB.write(Bdir); pwm_A.write(abs(for_A)); pwm_B.write(abs(for_B)); - //pc.printf(" %f %f %f %f \n\r",kbl,zy,vyuit,yuit); - //pc.printf(" %i %i %i\n\r",tickb,refB,errB); - //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); if(pc.txBufferGetSize(0)-pc.txBufferGetCount() > 100){ - pc.printf("B %i %i %i\n\r",tickb,refB,errB); + //pc.printf(" %f\n\r",zy); + //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("%i\n",motor2.getPosition()); + //pc.printf("%f\n\r",for_A); + pc.printf("%f\n\r",for_B); } } }