Eindversie. LU: 07-11-13.
Dependencies: MODSERIAL mbed Encoder
Revision 29:8600b02ab223, committed 2013-11-01
- Comitter:
- Socrates
- Date:
- Fri Nov 01 15:18:14 2013 +0000
- Parent:
- 28:2904487e0a1e
- Child:
- 30:c569058f10aa
- Commit message:
- Calibratie: werkt, op ??n punt.; Meten: gaat best ok, richtingpins moeten nog beter.; Patroon tekenen: werkt voor geen bal.
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Nov 01 13:01:08 2013 +0000 +++ b/main.cpp Fri Nov 01 15:18:14 2013 +0000 @@ -42,6 +42,7 @@ volatile bool meetflag=true; volatile bool calA, calB; volatile bool patroonflag=false; +volatile bool startflag=true; int main() { @@ -102,18 +103,18 @@ kd=0.1*2.8*pow(10.0,-3.0); ki=0.1*1.0; rt=0.032805; - gain=5.0; - emggrens=0.3; + gain=4.0; + emggrens=0.35; friction=0.4; Ai1=0; Ad1=0; Bi1=0; Bd1=0; pc.baud(115200); - Aboven=800; - Aonder=185; - Bboven=10800; - Bonder=3500; + Aboven=820; + Aonder=165; + Bboven=10900; + Bonder=3400; //Filtercoëfficienten. //High pass, 35Hz, 1e orde, 4 ms. @@ -138,13 +139,19 @@ //denl3=0.982385450614126; //Opzetje voor calibratie - motor1.setPosition(0); - motor2.setPosition(1200); - wait(6); + wait(3); calA=true; calB=true; +if(startflag==true); +{ + motor1.setPosition(0); + motor2.setPosition(1200); + startflag=false; + } + while(calflag==true) { + while(looptimerflag != true); looptimerflag = false; //515,3536 voor rechtsonder. @@ -160,7 +167,7 @@ Bi1=Bi; ctrlB=(Bi+Bp+Bd); for_B=(ctrlB)/1000.0; - if(ctrlB<0) { //Is nu omgedraaid. Was 0-1. + if(ctrlB<0) { Bdir=0; } else { Bdir=1; @@ -212,7 +219,7 @@ //Einde opzetje. //Loop. - wait(2); + wait(1); while(meetflag==true) { while(looptimerflag != true); { @@ -290,13 +297,13 @@ zy=zy*(1.0/(1.0-emggrens)); //Richting omdraaien met triceps. - if (ztr>0.1 && dirflagx == true) { + if (ztr>0.15 && dirflagx == true) { dirflagx = false; xdir ^= 1; zx=0; dirtimeout.attach(tricheck,1.0); } - if (ztl>0.1 && dirflagy == true) { + if (ztl>0.15 && dirflagy == true) { dirflagy = false; ydir ^= 1; zy=0; @@ -304,7 +311,7 @@ } //Motoraansturing. - if (ydir==1) { + if (ydir==0) { zy=-1.0*zy; } if (xdir==1) { @@ -316,14 +323,17 @@ //Begrenzing. vxuit=zx*4.0*pow(10.0,-1.0); // 4cm/s - vyuit=zy*4.0*pow(10.0,-1.0); // 4cm/s + vyuit=zy*4.0*pow(10.0,-2.0); // 4cm/s xuit += ts*vxuit; yuit += ts*vyuit; +keep_in_range(&xuit,-4.5,2.75); //Iets met de grenzen. En de richtingpins. +keep_in_range(&yuit,-4.5,2.8); + 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); errA=refA-ticka; errB=refB-tickb; @@ -351,9 +361,9 @@ //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks. if(ctrlA<0) { - Adir=0; + Adir=1; } else { - Adir=1; + Adir=0; } if(ctrlB<0) { Bdir=0; @@ -368,8 +378,8 @@ 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); + keep_in_range(&for_A, 0,1.0); + keep_in_range(&for_B, 0,1.0); } motordirA.write(Adir); motordirB.write(Bdir); @@ -377,9 +387,9 @@ pwm_B.write(abs(for_B)); if(pc.txBufferGetSize(0)-pc.txBufferGetCount() > 100) { - //pc.printf(" %f\n\r",zy); - 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(" %f %f \n\r",zx, zy); + //pc.printf(" %f %i %i %f %i %i\n\r",for_A,ticka,refA,for_B,tickb, refB); + pc.printf(" %i %i %i %i %f %f \n\r", ticka,tickb,refA,refB,xuit,yuit); //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); @@ -392,43 +402,38 @@ } while(patroonflag==true) { - - zx=400; - zy=140; - while(1) { + while(looptimerflag != true); + looptimerflag = false; + xuit=400.0; + yuit=140.0; + 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*1000.0); + errA=refA-ticka; + errB=refB-tickb; + while(errA>20 && errB>20) { 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); + Ap=errA*kpc; + Ad=(errA-Ad1)*kdc/ts; + Ai=(Ai1+ts*errA)*kic; 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); + for_A=(ctrlA)/1000.0; + Bp=errB*kpc; + Bd=(errB-Bd1)*kdc/ts; + Bi=(Bi1+ts*errB)*kic; 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. - + for_B=(ctrlB)/1000.0; if(ctrlA<0) { Adir=0; } else { @@ -439,22 +444,68 @@ } 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); + keep_in_range(&for_A, 0,0.08); + keep_in_range(&for_B, 0,0.1); } motordirA.write(Adir); motordirB.write(Bdir); pwm_A.write(abs(for_A)); pwm_B.write(abs(for_B)); - + } + wait(1); + xuit=400.0; + yuit=180.0; + while(errA>20 && errB>20) { + 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*1000.0); + //keep_in_rangeint(&refA,Aonder,Aboven); + //keep_in_rangeint(&refB,Bonder,Bboven); + errA=refA-ticka; + errB=refB-tickb; + //Controllers + 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; + 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(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.08); + keep_in_range(&for_B, 0,0.1); + } + motordirA.write(Adir); + motordirB.write(Bdir); + pwm_A.write(abs(for_A)); + pwm_B.write(abs(for_B)); } }