Eindversie. LU: 07-11-13.
Dependencies: MODSERIAL mbed Encoder
Revision 35:db3385976119, committed 2013-11-05
- Comitter:
- Socrates
- Date:
- Tue Nov 05 08:56:16 2013 +0000
- Parent:
- 34:961aec0a13c6
- Child:
- 36:3fad1225c3ad
- Commit message:
- Doet het best goed.
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Nov 04 16:26:02 2013 +0000 +++ b/main.cpp Tue Nov 05 08:56:16 2013 +0000 @@ -2,10 +2,7 @@ #include "MODSERIAL.h" #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. -//Rechts is x, links is y +//Rechterarm is x, linkerarm is y. //Inputs. AnalogIn emgtr(PTB3); @@ -45,8 +42,7 @@ volatile bool frictionflag=true; volatile bool calflag=true; -volatile bool meetflag=false; -volatile bool patroonflag=true; +volatile bool meetflag=true; int main() { @@ -65,12 +61,11 @@ double zx,zy, xuit,yuit, rt; double vxuit, vyuit, Ap, Ad, Ad1, kd, kp, ki, Ai, Ai1, Bp, Bd, Bd1, Bi, Bi1; double for_A, for_B, ctrlA, ctrlB, kdc, kpc, kic; - double gain, emggrens,friction; + double gain, emggrens,frictiona,frictionb, schrijfgainx, schrijfgainy; int xdir, ydir, Adir, Bdir; int ticka, tickb, refA,refB, errA, errB; int Aboven, Aonder, Bboven,Bonder; - - int ppos=1; + int Astart, Bstart; //Startwaarden. x1tr=0; y1tr=0; @@ -111,7 +106,12 @@ rt=0.032805; gain=4.0; emggrens=0.35; - friction=0.45; + frictiona=0.7; + frictionb=0.5; + schrijfgainx=0.1; + schrijfgainy=0.05; + Astart=000; + Bstart=1200; Ai1=0; Ad1=0; Bi1=0; @@ -137,17 +137,20 @@ denl2=-1.822694925196308; denl3=0.837181651256023; -//Opzetje voor calibratie +//Calibratie wait(4); if(startflag==true); { - motor1.setPosition(200); - motor2.setPosition(1100); + startflag=false; } while(calflag==true) { + wait(0.1); + motor1.setPosition(Astart); + wait(0.1); + motor2.setPosition(Bstart); while(looptimerflag != true); looptimerflag = false; //515 - 3536 voor rechtsonder. @@ -170,7 +173,7 @@ } keep_in_range(&for_B, -1.0,1.0); if (frictionflag==true) { - for_B=abs(for_B)+friction; + for_B=abs(for_B)+frictionb; keep_in_range(&for_B, 0.0,0.1); } pwm_B.write(abs(for_B)); @@ -198,7 +201,7 @@ } keep_in_range(&for_A, -1,1); if (frictionflag==true) { - for_A=abs(for_A)+friction; + for_A=abs(for_A)+frictiona; keep_in_range(&for_A, 0,0.1); } pwm_A.write(abs(for_A)); @@ -210,7 +213,6 @@ } } } -//Einde opzetje. //Loop. wait(1); @@ -322,15 +324,16 @@ ticka=-1*motor1.getPosition(); tickb=motor2.getPosition(); - //Begrenzing. - vxuit=zx*70.0/500.0; - vyuit=zy*50.0/500.0; + //Omzetten naar schrijfsnelheid. + vxuit=zx*schrijfgainx; + vyuit=zy*schrijfgainy; xuit += ts*vxuit; yuit += ts*vyuit; keep_in_range(&xuit,0.115,0.422); keep_in_range(&yuit,0.115,0.335); + 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); @@ -371,12 +374,12 @@ keep_in_range(&for_B, -1.0,1.0); if (frictionflag==true) { - for_A=abs(for_A)+friction; - for_B=abs(for_B)+friction; + for_A=abs(for_A)+frictiona; + for_B=abs(for_B)+frictionb; + keep_in_range(&for_A, frictiona,1); + keep_in_range(&for_B, frictionb,1); + } - keep_in_range(&for_A, friction,friction+0.2); - keep_in_range(&for_B, friction,friction+0.2); - } motordirA.write(Adir); motordirB.write(Bdir); pwm_A.write(abs(for_A)); @@ -397,91 +400,8 @@ //pc.printf("%f %f\n\r",for_A,for_B); } } - - while(patroonflag==true) { - while(looptimerflag != true); - looptimerflag = false; - - if(ppos==1) { - refA=221; - refB=8477; - //xuit=0.400; - //yuit=0.140; - } - if(ppos==2) { - refA=278; - refB=8774; - //xuit=0.400; - //yuit=0.180; - } - if(ppos==3) { - //xuit=0.400; - //yuit=0.200; - } - if(ppos==4) { - //xuit=0.340; - //yuit=0.200; - } - if(ppos==5) { - //xuit=0.340; - //yuit=0.180; - } - if(ppos==6) { - //xuit=0.350; - //yuit=0.200; - } - 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; - //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)/100.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)/100.0; - if(ctrlA<0) { - Adir=1; - } else { - Adir=0; - } - 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.5); - keep_in_range(&for_B, 0,0.6); - } - motordirA.write(Adir); - motordirB.write(Bdir); - pwm_A.write(abs(for_A)); - pwm_B.write(abs(for_B)); - if(errA<40 && errB<40) { - ppos +=1; - wait(1); - } - - } } - void keep_in_range(double * in, double min, double max) { *in > min ? *in < max? : *in = max: *in = min;