Eindversie. LU: 07-11-13.
Dependencies: MODSERIAL mbed Encoder
Revision 28:2904487e0a1e, committed 2013-11-01
- Comitter:
- Socrates
- Date:
- Fri Nov 01 13:01:08 2013 +0000
- Parent:
- 27:5d0c94b991aa
- Child:
- 29:8600b02ab223
- Commit message:
- Voor patroon. calibratie werkt. Aansturing nog niet echt.
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Nov 01 09:16:46 2013 +0000 +++ b/main.cpp Fri Nov 01 13:01:08 2013 +0000 @@ -20,9 +20,6 @@ Encoder motor1(PTD0,PTC9); Encoder motor2(PTD5,PTC8); -DigitalIn sluis1(PTE0); -DigitalIn sluis2(PTE1); - //Functies en flags. void keep_in_range(double * in, double min, double max); void keep_in_rangeint(int * in, int min, int max); @@ -40,9 +37,11 @@ dirflagy=true; } -volatile bool calflag=false; -volatile bool grensflag=false; +volatile bool calflag=true; volatile bool frictionflag=true; +volatile bool meetflag=true; +volatile bool calA, calB; +volatile bool patroonflag=false; int main() { @@ -60,10 +59,9 @@ 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; + double for_A, for_B, ctrlA, ctrlB, kdc, kpc, kic; + double gain, emggrens,friction; + int xdir, ydir, Adir, Bdir; int ticka, tickb, refA,refB, errA, errB; int Aboven, Aonder, Bboven,Bonder; //Startwaarden. @@ -97,22 +95,25 @@ ydir=0; xuit=0; yuit=0; - kp=1.0*0.1545; - kd=1.0*0.0*2.8*pow(10.0,-3.0); + kpc=1.0*0.1545; + kdc=0.0*2.8*pow(10.0,-3.0); + kic=0.1*1.0; + kp=0.1*0.1545; + kd=0.1*2.8*pow(10.0,-3.0); ki=0.1*1.0; rt=0.032805; gain=5.0; - grens=0.3; - friction=0.2; + emggrens=0.3; + friction=0.4; Ai1=0; Ad1=0; Bi1=0; Bd1=0; pc.baud(115200); - Aboven=500; - Aonder=-500; - Bboven=7000; - Bonder=-500; + Aboven=800; + Aonder=185; + Bboven=10800; + Bonder=3500; //Filtercoëfficienten. //High pass, 35Hz, 1e orde, 4 ms. @@ -136,47 +137,83 @@ //denl2=-1.982228929792529; //denl3=0.982385450614126; -//Tijdstap voor sinusinput -//double t; -//t=0; - //Opzetje voor calibratie - while(calflag==true) { - - Asluis=sluis1.read(); - Bsluis=sluis2.read(); + motor1.setPosition(0); + motor2.setPosition(1200); + wait(6); + calA=true; + calB=true; - 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); + while(calflag==true) { + while(looptimerflag != true); + looptimerflag = false; + //515,3536 voor rechtsonder. + refA=515; + refB=3536; + while(calB==true) { + tickb=motor2.getPosition(); + errB=refB-tickb; + Bp=errB*kpc; + Bd=(errB-Bd1)*kdc/ts; + Bi=(Bi1+ts*errB)*kic; + Bd1=Bd; + Bi1=Bi; + ctrlB=(Bi+Bp+Bd); + for_B=(ctrlB)/1000.0; + if(ctrlB<0) { //Is nu omgedraaid. Was 0-1. + Bdir=0; + } else { + Bdir=1; + } + keep_in_range(&for_B, -1,1); + if (frictionflag==true) { + for_B=abs(for_B)+friction; + keep_in_range(&for_B, 0,0.1); + } + pwm_B.write(abs(for_B)); + motordirB.write(Bdir); + //pc.printf("B %f %i \n\r",for_B,errB); + if(errB<10) { + calB=false; + pwm_B.write(0); + } } - 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_B.write(calB); + while(calA==true) { + ticka=-motor1.getPosition(); + errA=refA-ticka; + Ap=errA*kpc; + Ad=(errA-Ad1)*kdc/ts; + Ai=(Ai1+ts*errA)*kic; + Ad1=Ad; + Ai1=Ai; + ctrlA=(Ai+Ap+Ad); + for_A=(ctrlA)/1000.0; + if(ctrlA<0) { + Adir=1; + } else { + Adir=0; + } + keep_in_range(&for_A, -1,1); + if (frictionflag==true) { + for_A=abs(for_A)+friction; + keep_in_range(&for_A, 0,0.08); + } + pwm_A.write(abs(for_A)); + motordirA.write(Adir); + //pc.printf("A %f %i \n\r",for_A,errA); + if(errA<20) { + calA=false; + pwm_A.write(0); + calflag=false; + } } - pwm_B.write(0); - - motor1.setPosition(0); - motor2.setPosition(0); - calflag=false; } //Einde opzetje. //Loop. -wait(2); - while(1) { + wait(2); + while(meetflag==true) { while(looptimerflag != true); { } @@ -234,23 +271,23 @@ zx=(zbr*gain); zy=(zbl*gain); -//Grenzen. +//Grenzen voor emg. if (zx>1.0) { zx=0.99999; } if (zy>1.0) { zy=0.99999; } - if (zx<grens) { - zx=grens; + if (zx<emggrens) { + zx=emggrens; } - if (zy<grens) { - zy=grens; + if (zy<emggrens) { + zy=emggrens; } - zx=zx-grens; - zx=zx*(1.0/(1.0-grens)); - zy=zy-grens; - zy=zy*(1.0/(1.0-grens)); + zx=zx-emggrens; + zx=zx*(1.0/(1.0-emggrens)); + zy=zy-emggrens; + zy=zy*(1.0/(1.0-emggrens)); //Richting omdraaien met triceps. if (ztr>0.1 && dirflagx == true) { @@ -274,67 +311,23 @@ zx=-1.0*zx; } - ticka=motor1.getPosition(); + ticka=-1*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); - 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; @@ -344,7 +337,7 @@ Ad1=Ad; Ai1=Ai; ctrlA=(Ai+Ap+Ad); - for_A=(ctrlA)/1000.0; + for_A=(ctrlA)/700.0; Bp=errB*kp; Bd=(errB-Bd1)*kd/ts; @@ -354,7 +347,7 @@ Bd1=Bd; Bi1=Bi; ctrlB=(Bi+Bp+Bd); - for_B=(ctrlB)/1000.0; + for_B=(ctrlB)/7000.0; //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks. if(ctrlA<0) { @@ -368,17 +361,16 @@ Bdir=1; } - keep_in_range(&for_A, -1,1); - keep_in_range(&for_B, -1,1); - - 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); - } + keep_in_range(&for_A, -1.0,1.0); + keep_in_range(&for_B, -1.0,1.0); + + if (frictionflag==true) { + for_A=abs(for_A)+friction; + for_B=abs(for_B)+friction; + + keep_in_range(&for_A, 0,0.5); + keep_in_range(&for_B, 0,0.5); + } motordirA.write(Adir); motordirB.write(Bdir); pwm_A.write(abs(for_A)); @@ -386,17 +378,86 @@ 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(" %f %f %i %i %i %i\n\r",for_A,for_B, refA,refB,errA,errB); + //pc.printf(" %i %i %i %i \n\r", ticka,tickb,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("tr %f br %f tl %f bl %f\n\r",ztr,zbr,ztl,zbl); + //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 %f\n\r",for_A,for_B); } } + + while(patroonflag==true) { + + zx=400; + zy=140; + while(1) { + ticka=-1*motor1.getPosition(); + tickb=motor2.getPosition(); + + 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); + + errA=refA-ticka; + errB=refB-tickb; + + //Controllers + Ap=errA*kp; + Ad=(errA-Ad1)*kd/ts; + Ai=(Ai1+ts*errA)*ki; + //keep_in_range(&Ad,-0.1,0.1); + //keep_in_range(&Ai,-0.1,0.1); + Ad1=Ad; + Ai1=Ai; + ctrlA=(Ai+Ap+Ad); + for_A=(ctrlA)/700.0; + + Bp=errB*kp; + Bd=(errB-Bd1)*kd/ts; + Bi=(Bi1+ts*errB)*ki; + //keep_in_range(&Bd,-0.1,0.1); + //keep_in_range(&Bi,-0.1,0.1); + Bd1=Bd; + Bi1=Bi; + ctrlB=(Bi+Bp+Bd); + for_B=(ctrlB)/7000.0; + //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks. + + if(ctrlA<0) { + Adir=0; + } else { + Adir=1; + } + if(ctrlB<0) { + Bdir=0; + } else { + Bdir=1; + } + + keep_in_range(&for_A, -1.0,1.0); + keep_in_range(&for_B, -1.0,1.0); + + if (frictionflag==true) { + for_A=abs(for_A)+friction; + for_B=abs(for_B)+friction; + + keep_in_range(&for_A, 0,0.5); + keep_in_range(&for_B, 0,0.5); + } + motordirA.write(Adir); + motordirB.write(Bdir); + pwm_A.write(abs(for_A)); + pwm_B.write(abs(for_B)); + + } + } + } void keep_in_range(double * in, double min, double max)