Eindversie. LU: 07-11-13.
Dependencies: MODSERIAL mbed Encoder
Revision 22:a658a3e7b3f4, committed 2013-10-29
- Comitter:
- Socrates
- Date:
- Tue Oct 29 13:44:03 2013 +0000
- Parent:
- 21:659f7c8ed328
- Child:
- 23:ee89be59ae2f
- Commit message:
- Doet niet.
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Oct 29 12:17:41 2013 +0000 +++ b/main.cpp Tue Oct 29 13:44:03 2013 +0000 @@ -3,8 +3,8 @@ #include "encoder.h" #define PI 3.14159265358979323 //XenY -//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. +//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. //Rechts is x, links is y AnalogIn emgtr(PTB3); AnalogIn emgbr(PTB2); @@ -23,7 +23,7 @@ volatile bool looptimerflag; void setlooptimerflag(void) { - looptimerflag = true; +looptimerflag = true; } volatile bool dirflagx=true; @@ -31,8 +31,8 @@ void tricheck(void) { - dirflagx=true; - dirflagy=true; +dirflagx=true; +dirflagy=true; } int main() @@ -49,61 +49,28 @@ 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 xuit,yuit, rt; - //float zx1, zy1, gain; - //float pwmA, pwmB; + float xuit,yuit, rt; int xdir, ydir, Adir, Bdir; float vxuit, vyuit, Ap, Ad, Ad1, kd, kp, ki, Ai, Ai1, Bp, Bd, Bd1, Bi, Bi1; - float for_A, for_B, yuit1, xuit1; + float for_A, for_B; int ticka, tickb, refA,refB, errA, errB; float ctrlA; float ctrlB; //Startwaarden - 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; - xuit=0; - yuit=0; - kp=0.1; - ki=0.01; - kd=0.01; - rt=0.032805; - Bi1=0; - Bd1=0; - yuit1=0; - xuit1=0; + 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; xuit=0; yuit=0; + kp=1; ki=0.0; kd=0.0; rt=0.032805; Ai1=0; Ad1=0; + Bi1=0; Bd1=0; //High pass, 35Hz, 1e orde numh1=0.900575535279376; numh2=-0.900575535279376; //denh1=1; denh2=-0.801151070558751; - + //Low pass, 5 Hz, 2e orde. numl1=0.241359049041961*pow(10.0,-3.0); numl2=0.482718098083923*pow(10.0,-3.0); @@ -114,63 +81,42 @@ pc.baud(115200); while(1) { - while(looptimerflag != true); - { + while(looptimerflag != true);{ } looptimerflag = false; //EMG lezen. - ktr=emgtr.read(); +ktr=emgtr.read(); xtr=(ktr-0.5)*2.0; ytr=xtr*numh1+x1tr*numh2-y1tr*denh2; yabstr=abs(ytr); ztr=yabstr*numl1+yabs1tr*numl2+yabs2tr*numl3-z1tr*denl2-z2tr*denl3; - x1tr=xtr; - y1tr=ytr; - z2tr=z1tr; - z1tr=ztr; - yabs2tr=yabs1tr; - yabs1tr=yabstr; + x1tr=xtr; y1tr=ytr; z2tr=z1tr; z1tr=ztr; yabs2tr=yabs1tr; yabs1tr=yabstr; - kbr=emgbr.read(); +kbr=emgbr.read(); xbr=(kbr-0.5)*2.0; ybr=xbr*numh1+x1br*numh2-y1br*denh2; yabsbr=abs(ybr); zbr=yabsbr*numl1+yabs1br*numl2+yabs2br*numl3-z1br*denl2-z2br*denl3; - x1br=xbr; - y1br=ybr; - z2br=z1br; - z1br=zbr; - yabs2br=yabs1br; - yabs1br=yabsbr; + x1br=xbr; y1br=ybr; z2br=z1br; z1br=zbr; yabs2br=yabs1br; yabs1br=yabsbr; - ktl=emgtl.read(); +ktl=emgtl.read(); xtl=(ktl-0.5)*2.0; ytl=xtl*numh1+x1tl*numh2-y1tl*denh2; yabstl=abs(ytl); ztl=yabstl*numl1+yabs1tl*numl2+yabs2tl*numl3-z1tl*denl2-z2tl*denl3; - x1tl=xtl; - y1tl=ytl; - z2tl=z1tl; - z1tl=ztl; - yabs2tl=yabs1tl; - yabs1tl=yabstl; + x1tl=xtl; y1tl=ytl; z2tl=z1tl; z1tl=ztl; yabs2tl=yabs1tl; yabs1tl=yabstl; - kbl=emgbl.read(); +kbl=emgbl.read(); xbl=(kbl-0.5)*2.0; ybl=xbl*numh1+x1bl*numh2-y1bl*denh2; 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; + x1bl=xbl; y1bl=ybl; z2bl=z1bl; z1bl=zbl; yabs2bl=yabs1bl; yabs1bl=yabsbl; - zx=(zbr*5.0); - zy=(zbl*5.0); - + zx=(zbr*3.0); + zy=(zbl*3.0); + //Grenzen. if (zx>1.0) { zx=0.99999; @@ -178,94 +124,91 @@ if (zy>1.0) { zy=0.99999; } - if (zx<0.20) { - zx=0; + if (zx<0.30){ + zx=0; } - if (zy<0.20) { - zy=0; + if (zy<0.30){ + zy=0; } - - //Richting omdraaien met triceps. + + //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) { + if ((ztl>(zbl+0.1)) && dirflagy == true) { dirflagy = false; ydir ^= 1; zy=0; dirtimeout.attach(tricheck,1.5); } - + //Motoraansturing. - if (ydir==1) { - zy=-1.0*zy; + if (ydir==1) + { + zy=-1.0*zy; } - if (xdir==1) { - zx=-1.0*zx; + if (xdir==1) + { + zx=-1.0*zx; } - - 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 + + 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 xuit += ts*vxuit; yuit += ts*vyuit; - yuit1=yuit; - + refA=floor(4123.0*atan2(yuit,xuit)/(2.0*PI)); refB=floor(4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt)); errA=refA-ticka; - errB=refB-tickb; - - if(errA<0) { - Adir=0; - } else { - Adir=1; - } - if(errB<0) { - Bdir=0; - } else { - Bdir=1; - } + errB=refB-tickb; + +if(errA<0){ +Adir=0;} +else{ +Adir=1; +} +if(errB<0){ +Bdir=0;} +else{ +Bdir=1; +} //Controllers - Ap=errA*kp; - Ad=(errA-Ad1)*kd/ts; - Ai=(Ai1+ts*errA)*ki; - keep_in_range(&Ai, -0.1,0.1); - Ad1=Ad; - Ai1=Ai; - ctrlA=Ai+Ap+Ad; - for_A = ctrlA/1000.; + 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); + Ad1=Ad; Ai1=Ai; + ctrlA=(Ai+Ap+Ad); + for_A=(ctrlA)/1000.0; - errB= 1; - Bp=errB*kp; - Bd=(errB-Bd1)*kd/ts; - Bi=(Bi1+ts*errB)*ki; - keep_in_range(&Bi, -0.1,0.1); - Bd1=Bd; - Bi1=Bi; - ctrlB=Bi+Bp+Bd; - for_B= ctrlB / 1000.0; - //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks. - + 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); + Bd1=Bd; Bi1=Bi; + ctrlB=(Bi+Bp+Bd); + for_B=(ctrlB)/1000.0; + //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks. + keep_in_range(&for_A, -1,1); keep_in_range(&for_B, -1,1); motordirA.write(Adir); motordirB.write(Bdir); - 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(" %i %i %i %f %f\n\r",tickb,refB,errB,ctrlB,for_B); - //pc.printf("b %f t %f xu %f yu %f A %f B %f \n\r",zbl,ztl,xuit, yuit, pwmA,pwmB); + 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); } } void keep_in_range(float * in, float min, float max) { -*in > min ? *in < max? : *in = max: *in = min; + *in > min ? *in < max? : *in = max: *in = min; } \ No newline at end of file