Eindversie. LU: 07-11-13.
Dependencies: MODSERIAL mbed Encoder
Revision 19:09c4b5249cec, committed 2013-10-28
- Comitter:
- Socrates
- Date:
- Mon Oct 28 16:09:13 2013 +0000
- Parent:
- 18:6c0200364678
- Child:
- 20:013e9c00e058
- Commit message:
- Goede poging tot regelaar.
Changed in this revision
Encoder.lib | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Encoder.lib Mon Oct 28 16:09:13 2013 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/vsluiter/code/Encoder/#2dd7853c911a
--- a/main.cpp Fri Oct 25 12:36:02 2013 +0000 +++ b/main.cpp Mon Oct 28 16:09:13 2013 +0000 @@ -1,5 +1,11 @@ #include "mbed.h" #include "MODSERIAL.h" +#include "encoder.h" +#define PI 3.14159265358979323 + +//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. + //XenY AnalogIn emgtr(PTB3); AnalogIn emgbr(PTB2); @@ -11,7 +17,8 @@ MODSERIAL pc(USBTX,USBRX); DigitalOut motordirA(PTD3); DigitalOut motordirB(PTD1); - +Encoder motor1(PTD0,PTC9); +Encoder motor2(PTD5,PTC8); void keep_in_range(float * in, float min, float max); @@ -44,17 +51,19 @@ 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 gainb, xuit, kp, ki, zx1, yuit, zy1, pwmA, pwmB,rt; + float gain, xuit, kp, ki, zx1, yuit, zy1, pwmA, pwmB,rt; int xdir; int ydir; + float vxuit, vyuit, Ap, Ad, Ad1, kd, kp, ki, Ai, Ai1, Bp, Bd, Bd1, Bi, Bi1; + float for_A, for_B; + int ticka, tickb, refA,refB, errA, errB, x1tr=0; y1tr=0; z1tr=0; z2tr=0; yabs1tr=0; yabs2tr=0; x1br=0; y1br=0; z1br=0; z2br=0; yabs1br=0; yabs2br=0; 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; - gainb=3.0; kp=0; ki=1; zx1=0; zy1=0; rt=33.0/1000.0; + zx=0; zy=0; xdir=0; ydir=0; + kp=0; ki=1; zx1=0; zy1=0; rt=32.805/1000.0; //High pass, 35Hz, 1e @@ -81,6 +90,7 @@ } looptimerflag = false; +//EMG lezen. ktr=emgtr.read(); xtr=(ktr-0.5)*2.0; ytr=xtr*numh1+x1tr*numh2-y1tr*denh2; @@ -108,17 +118,17 @@ yabsbl=abs(ybl); 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); + zy=(zbl); - zx=(zbr*gainb); - zy=(zbl*gainb); - + //Grenzen. if (zx>1.0) { zx=0.99999; } if (zy>1.0) { zy=0.99999; } - if (zx<0.30){ zx=0; } @@ -126,36 +136,55 @@ zy=0; } + //Richting omdraaien met triceps. if ((ztr>(zbr+0.1)) && dirflagx == true) { dirflagx = false; xdir ^= 1; - zx=0; dirtimeout.attach(tricheck,1.5); } if ((ztl>(zbl+0.1)) && dirflagy == true) { dirflagy = false; ydir ^= 1; - zy=0; dirtimeout.attach(tricheck,1.5); } - xuit=(kp*zx+ki*zx1)/(kp+ki); - yuit=(kp*zy+ki*zy1)/(kp+ki); - //pwmA=(atan(yuit/(xuit+0.0001)))/(2.0*3.14159265359); - //pwmB=(sqrt(xuit*xuit+yuit*yuit))/(2.0*3.14159265359*rt); - //keep_in_range(&pwmA, -1,1); - //keep_in_range(&pwmB, -1,1); + + //Motoraansturing. + if (ydir==1) + { + zy=-1.0*zy; + } + if (xdir==1) + { + zx=-1.0*zx; + } + + ticka=motor1.getPosition(); tickb=motor2.getPosition(); + vxuit=zx*4.0*pow(10.0,-5.0); // 4cm/s + vyuit=zy*4.0*pow(10.0,-5.0); // 4cm/s + xuit += ts*vxuit; + yuiy += ts*vyuit; + + refA=4123.0*atan2(yuit,xuit)/(2.0*PI); + refB=4123.0*sqrt(xuit*xuit+yuit*yuit)*(2.0*PI*rt);//nog keer omtrek + errA=refA-ticka; + errB=refB-tickb; - zx1=zx; - zy1=zy; + //Controllers + Ap=errA*kp; Ad=(errA-Ad1)*kd/ts; Ai=(Ai1+ts*errA)*ki; + Ad1=Ad; Ai1=Ai; + for_a=Ai+Ap+Ad; + Bp=errB*kp; Bd=(errB-Bd1)*kd/ts; Bi=(Bi1+ts*errB)*ki; + Bd1=Bd; Bi1=Bi; + for_B=Bi+Bp+Bd; + //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks. - if (pwmA<0.0002) - { - pwmA=0; - } + keep_in_range(&for_A, -1,1); + keep_in_range(&for_B, -1,1); + motordirA.write(xdir); motordirB.write(ydir); - pwm_A.write((yuit*3.0)); - pwm_B.write((yuit*3.0)); + pwm_A.write(for_A); + pwm_B.write(for_B); //pc.printf("Bi: %f, Tri: %f, Richting: %d \n\r",zbl*3.0,ztl*3.0,ydir); pc.printf(" %f\n\r",zy);