Eindversie. LU: 07-11-13.
Dependencies: MODSERIAL mbed Encoder
Revision 24:2e5290d8230b, committed 2013-10-30
- Comitter:
- Socrates
- Date:
- Wed Oct 30 13:02:45 2013 +0000
- Parent:
- 23:ee89be59ae2f
- Child:
- 25:bfe7c49e76cd
- Commit message:
- voor victor
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Oct 29 14:41:05 2013 +0000 +++ b/main.cpp Wed Oct 30 13:02:45 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=0.1; ki=0.0001; kd=0.0001; rt=0.032805; Ai1=0; Ad1=0; + kp=0.1; ki=0.0000; kd=0.0000; rt=0.032805; Ai1=0; Ad1=0; Bi1=0; Bd1=0; //High pass, 35Hz, 1e orde @@ -78,6 +78,14 @@ //denl1=1; denl2=-1.955578240315036; denl3=0.956543676511203; + //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); + //denl1=1; + //denl2=-1.982228929792529; + //denl3=0.982385450614126; + pc.baud(115200); while(1) { @@ -114,8 +122,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*3.0); - zy=(zbl*3.0); + zx=(zbr*5.0); + zy=(zbl*5.0); //Grenzen. if (zx>1.0) { @@ -124,25 +132,28 @@ if (zy>1.0) { zy=0.99999; } - if (zx<0.30){ - zx=0; + if (zx<0.3){ + zx=0.3; } - if (zy<0.30){ - zy=0; + if (zy<0.3){ + zy=0.3; } - + zx=zx-0.3; + zx=zx*(1.0/0.7); + zy=zy-0.3; + zy=zy*(1.0/0.7); //Richting omdraaien met triceps. if ((ztr>(zbr+0.1)) && dirflagx == true) { dirflagx = false; xdir ^= 1; zx=0; - dirtimeout.attach(tricheck,1.5); + dirtimeout.attach(tricheck,1.0); } if ((ztl>(zbl+0.1)) && dirflagy == true) { dirflagy = false; ydir ^= 1; zy=0; - dirtimeout.attach(tricheck,1.5); + dirtimeout.attach(tricheck,1.0); } //Motoraansturing. @@ -156,8 +167,8 @@ } ticka=motor1.getPosition(); tickb=motor2.getPosition(); - vxuit=zx*1.0*pow(1.0,-2.0); // 4cm/s - vyuit=zy*1.0*pow(1.0,-2.0); // 4cm/s + 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; @@ -182,15 +193,14 @@ //keep_in_range(&Ai,-0.1,0.1); Ad1=Ad; Ai1=Ai; ctrlA=(Ai+Ap+Ad); - for_A=(ctrlA)/100.0; + for_A=(ctrlA)/10.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)/100.0; + for_B=(ctrlB)/10.0; //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks. keep_in_range(&for_A, -1,1); @@ -201,8 +211,14 @@ 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(" %i %i %i %f %f\n\r",tickb,refB,errB,ctrlB,for_B); + //pc.printf("A %i %i %i %i %f %f\n\r",ticka,refA,errA,ctrlA,for_A); + pc.printf("B %i %i %i %i %i %f %f\n\r",Bdir,tickb,refB,errB,ctrlB,for_B); +//Conclusies: tot en met yuit gaat het goed. Kabel A lijkt het beter te doen dan kabel B. Als de EMG niet aangesloten zit is er nog steeds ruis van de +//encoder. + } }