![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Eindversie. LU: 07-11-13.
Dependencies: MODSERIAL mbed Encoder
Diff: main.cpp
- Revision:
- 37:c6d753123173
- Parent:
- 36:3fad1225c3ad
- Child:
- 38:fb5c45d85ead
--- a/main.cpp Wed Nov 06 10:48:02 2013 +0000 +++ b/main.cpp Thu Nov 07 12:23:00 2013 +0000 @@ -37,16 +37,15 @@ dirflagy=true; } -volatile bool startflag=true; -volatile bool calA=true, calB=true; -volatile bool frictionflag=true; +volatile bool calA=true, calB=true; //Calibratie per motor. +volatile bool frictionflag=true; //Wrijvingscompensatie. -volatile bool calflag=true; -volatile bool meetflag=true; +volatile bool calflag=true; //Calibreren? +volatile bool meetflag=true; //Meten? int main() { -//Constantes en tickers. +//Constanten en tickers. pwm_A.period(1.0/2500.0); pwm_B.period(1.0/2500.0); Ticker looptimer; @@ -62,10 +61,8 @@ 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,frictiona,frictionb, schrijfgainx, schrijfgainy; - int xdir, ydir, Adir, Bdir; - int ticka, tickb, refA,refB, errA, errB; - int Aboven, Aonder, Bboven,Bonder; - int Astart, Bstart; + int xdir, ydir, Adir, Bdir, ticka, tickb, refA,refB, errA, errB; + int Aboven, Aonder, Bboven,Bonder, Astart, Bstart; //Startwaarden. x1tr=0; y1tr=0; @@ -103,15 +100,15 @@ kp=1*0.1545; kd=1.0*2.8*pow(10.0,-3.0); ki=0.05*1.0; - rt=0.032805; - gain=4.0; + rt=0.032805; //Straal tandwiel. + gain=4.0; //Tegen filterverlies. emggrens=0.35; frictiona=0.65; frictionb=0.5; schrijfgainx=0.1; schrijfgainy=0.05; - Astart=000; - Bstart=1200; + Astart=0; + Bstart=1200; //Beginpositie pen in ticks. Ai1=0; Ad1=0; Bi1=0; @@ -120,7 +117,7 @@ Aboven=820; Aonder=165; Bboven=10900; - Bonder=3400; + Bonder=3400; //Grenzen in ticks. Komen overeen met hoeken van A4. //Filtercoëfficienten. //High pass, 35Hz, 1e orde, 4 ms. @@ -139,23 +136,13 @@ //Calibratie wait(4); - - if(startflag==true); - { - - 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. refA=515; - refB=3536; + refB=3536; //515 - 3536 voor rechtsonder. while(calB==true) { tickb=motor2.getPosition(); errB=refB-tickb; @@ -174,18 +161,19 @@ keep_in_range(&for_B, -1.0,1.0); if (frictionflag==true) { for_B=abs(for_B)+frictionb; - keep_in_range(&for_B, 0.0,0.1); + keep_in_range(&for_B, 0.0,0.1); + //Vreemd dat een bovengrens van 0.1 werkt ook al is friction groter dan 0.1. Bij hogere waarden gaat de motor te snel. } pwm_B.write(abs(for_B)); motordirB.write(Bdir); - if(errB<90) { + if(errB<20) { calB=false; pwm_B.write(0.0); } } while(calA==true) { - ticka=-1*motor1.getPosition(); + ticka=-1*motor1.getPosition(); //Omdat de motor op zijn kop staat. errA=refA-ticka; Ap=errA*kpc; Ad=(errA-Ad1)*kdc/ts; @@ -214,17 +202,16 @@ } } -//Loop. +//Meetloop. wait(1); - blueled.write(1.0); //motor1.setPosition(515); //motor2.setPosition(3565); + //Bovenstaande toegevoegd voor het geval de calibratie handmatig gedaan wordt. while(meetflag==true) { while(looptimerflag != true); - { - } looptimerflag = false; //EMG lezen. + //tr = triceps rechts. ktr=emgtr.read(); xtr=(ktr-0.5)*2.0; ytr=xtr*numh1+x1tr*numh2-y1tr*denh2; @@ -277,7 +264,7 @@ zx=(zbr*gain); zy=(zbl*gain); -//Grenzen voor emg. +//Grenzen en threshold voor emg. if (zx>1.0) { zx=0.99999; } @@ -290,10 +277,11 @@ if (zy<emggrens) { zy=emggrens; } + //Schaling om ook outputs lager dan de grenswaarde te krijgen. zx=zx-emggrens; - zx=zx*(1.0/(1.0-emggrens)); + zx=zx/(1.0-emggrens); zy=zy-emggrens; - zy=zy*(1.0/(1.0-emggrens)); + zy=zy/(1.0-emggrens); //Richting omdraaien met triceps. if (ztr>0.15 && dirflagx == true) { @@ -307,7 +295,7 @@ dirtimeout.attach(tricheck,1.0); } - //Motoraansturing. + //Bepalen juiste teken van verplaatsing. if (ydir==0) { zy=-1.0*zy; redled.write(1); @@ -323,28 +311,28 @@ greenled.write(0); } - ticka=-1*motor1.getPosition(); + ticka=-1*motor1.getPosition(); //Zie calibratie. tickb=motor2.getPosition(); //Omzetten naar schrijfsnelheid. vxuit=zx*schrijfgainx; vyuit=zy*schrijfgainy; xuit += ts*vxuit; - yuit += ts*vyuit; + yuit += ts*vyuit; //Integreren naar positie. + //Begrenzing penpositie, getallen in meter. keep_in_range(&xuit,0.115,0.422); keep_in_range(&yuit,0.115,0.335); - + //Inversie kinematica en begrenzing motorhoeken. 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); keep_in_rangeint(&refB,Bonder,Bboven); + //Controllers errA=refA-ticka; errB=refB-tickb; - - //Controllers Ap=errA*kp; Ad=(errA-Ad1)*kd/ts; Ai=(Ai1+ts*errA)*ki; @@ -361,6 +349,7 @@ ctrlB=(Bi+Bp+Bd); for_B=(ctrlB)/1500.0; + //Motorrichting bepalen. if(ctrlA<0) { Adir=1; } else { @@ -372,9 +361,11 @@ Bdir=1; } + //PWM-begrenzing. keep_in_range(&for_A, -1.0,1.0); keep_in_range(&for_B, -1.0,1.0); + //Wrijvingscompensatie. if (frictionflag==true) { for_A=abs(for_A)+frictiona; for_B=abs(for_B)+frictionb; @@ -387,6 +378,8 @@ pwm_A.write(abs(for_A)); pwm_B.write(abs(for_B)); + + //Verscheidene prints die nuttig zijn gebeleken. if(pc.txBufferGetSize(0)-pc.txBufferGetCount() > 100) { //pc.printf(" %f %f \n\r",zx, zy); pc.printf(" %f %f %f %f\n\r",zbl,ztl,zbr,ztr);