Eindversie. LU: 07-11-13.
Dependencies: MODSERIAL mbed Encoder
Revision 23:ee89be59ae2f, committed 2013-10-29
- Comitter:
- Socrates
- Date:
- Tue Oct 29 14:41:05 2013 +0000
- Parent:
- 22:a658a3e7b3f4
- Child:
- 24:2e5290d8230b
- Commit message:
- Code werkt grotendeels, maar rare sprongen bij encoder.
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Oct 29 13:44:03 2013 +0000 +++ b/main.cpp Tue Oct 29 14:41:05 2013 +0000 @@ -62,7 +62,7 @@ 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; xuit=0; yuit=0; - kp=1; ki=0.0; kd=0.0; rt=0.032805; Ai1=0; Ad1=0; + kp=0.1; ki=0.0001; kd=0.0001; rt=0.032805; Ai1=0; Ad1=0; Bi1=0; Bd1=0; //High pass, 35Hz, 1e orde @@ -156,13 +156,13 @@ } ticka=motor1.getPosition(); tickb=motor2.getPosition(); - vxuit=zx*1.0*pow(1.0,1.0); // 4cm/s - vyuit=zy*1.0*pow(1.0,1.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; yuit += ts*vyuit; - refA=floor(4123.0*atan2(yuit,xuit)/(2.0*PI)); - refB=floor(4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt)); + refA=(4123.0*atan2(yuit,xuit)/(2.0*PI)); + refB=(4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt)); errA=refA-ticka; errB=refB-tickb; @@ -178,18 +178,19 @@ } //Controllers Ap=errA*kp; Ad=(errA-Ad1)*kd/ts; Ai=(Ai1+ts*errA)*ki; - keep_in_range(&Ai,-0.1,0.1); - keep_in_range(&Ad,-0.1,0.1); + //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)/1000.0; + for_A=(ctrlA)/100.0; Bp=errB*kp; Bd=(errB-Bd1)*kd/ts; Bi=(Bi1+ts*errB)*ki; - keep_in_range(&Bi,-0.1,0.1); - keep_in_range(&Bd,-0.1,0.1); + //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)/1000.0; + for_B=(ctrlB)/100.0; //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks. keep_in_range(&for_A, -1,1); @@ -200,11 +201,8 @@ pwm_A.write(abs(for_A)); pwm_B.write(abs(for_B)); - //Voor EMG - pc.printf(" %f \n\r", zy); - - //Voor motor - //pc.printf(" %i %i %i %f %f\n\r",tickb,refB,errB,ctrlB,for_B); + //pc.printf(" %f %f %f %f \n\r",kbl,zy,vyuit, yuit); + pc.printf(" %i %i %i %f %f\n\r",tickb,refB,errB,ctrlB,for_B); } }