Thomas Plaisier / Mbed 2 deprecated G3_Barry_Plotter.

Eindversie. LU: 07-11-13.

Dependencies:   MODSERIAL mbed Encoder

main.cpp@38:fb5c45d85ead, 2013-11-07 (annotated)

Committer:
Socrates
Date:
Thu Nov 07 12:24:41 2013 +0000
Revision:
38:fb5c45d85ead
Parent:
37:c6d753123173 
Eindversie.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Socrates 0:b13a317308d1 1 #include "mbed.h"
Socrates 0:b13a317308d1 2 #include "MODSERIAL.h"
Socrates 19:09c4b5249cec 3 #include "encoder.h"
Socrates 19:09c4b5249cec 4 #define PI 3.14159265358979323
Socrates 35:db3385976119 5 //Rechterarm is x, linkerarm is y.
Socrates 26:539f131cf07c 6
Socrates 38:fb5c45d85ead 7 //Scripts voor de Barry Plotter. Gemaakt door Groep 3, BMT K9, 2013-2014.
Socrates 38:fb5c45d85ead 8 //Wouter Doppenberg, Gerard Essink, Ralph Lentink, Thomas Plaisier, Linda Zwiers.
Socrates 38:fb5c45d85ead 9 //Laatste wijziging: 07-11-13.
Socrates 38:fb5c45d85ead 10
Socrates 26:539f131cf07c 11 //Inputs.
Socrates 8:43cce9f7a006 12 AnalogIn emgtr(PTB3);
Socrates 8:43cce9f7a006 13 AnalogIn emgbr(PTB2);
Socrates 8:43cce9f7a006 14 AnalogIn emgtl(PTB1);
Socrates 8:43cce9f7a006 15 AnalogIn emgbl(PTB0);
Socrates 20:013e9c00e058 16 PwmOut pwm_A(PTA12);
Socrates 20:013e9c00e058 17 PwmOut pwm_B(PTA5);
Socrates 33:c495e9d8ea1f 18 PwmOut redled(LED_RED);
Socrates 33:c495e9d8ea1f 19 PwmOut greenled(LED_GREEN);
Socrates 33:c495e9d8ea1f 20 PwmOut blueled(LED_BLUE);
Socrates 0:b13a317308d1 21 MODSERIAL pc(USBTX,USBRX);
Socrates 17:7dd6dc3c7902 22 DigitalOut motordirA(PTD3);
Socrates 17:7dd6dc3c7902 23 DigitalOut motordirB(PTD1);
Socrates 19:09c4b5249cec 24 Encoder motor1(PTD0,PTC9);
Socrates 19:09c4b5249cec 25 Encoder motor2(PTD5,PTC8);
Socrates 20:013e9c00e058 26
Socrates 26:539f131cf07c 27 //Functies en flags.
Socrates 27:5d0c94b991aa 28 void keep_in_range(double * in, double min, double max);
Socrates 27:5d0c94b991aa 29 void keep_in_rangeint(int * in, int min, int max);
Socrates 18:6c0200364678 30
Socrates 0:b13a317308d1 31 volatile bool looptimerflag;
Socrates 0:b13a317308d1 32 void setlooptimerflag(void)
Socrates 0:b13a317308d1 33 {
Socrates 25:bfe7c49e76cd 34 looptimerflag = true;
Socrates 0:b13a317308d1 35 }
Socrates 17:7dd6dc3c7902 36 volatile bool dirflagx=true;
Socrates 17:7dd6dc3c7902 37 volatile bool dirflagy=true;
Socrates 13:e0e9fda0e9a1 38 void tricheck(void)
Socrates 13:e0e9fda0e9a1 39 {
Socrates 25:bfe7c49e76cd 40 dirflagx=true;
Socrates 25:bfe7c49e76cd 41 dirflagy=true;
Socrates 13:e0e9fda0e9a1 42 }
Socrates 13:e0e9fda0e9a1 43
Socrates 37:c6d753123173 44 volatile bool calA=true, calB=true; //Calibratie per motor.
Socrates 37:c6d753123173 45 volatile bool frictionflag=true; //Wrijvingscompensatie.
Socrates 30:c569058f10aa 46
Socrates 37:c6d753123173 47 volatile bool calflag=true; //Calibreren?
Socrates 37:c6d753123173 48 volatile bool meetflag=true; //Meten?
Socrates 27:5d0c94b991aa 49
Socrates 0:b13a317308d1 50 int main()
Socrates 0:b13a317308d1 51 {
Socrates 37:c6d753123173 52 //Constanten en tickers.
Socrates 26:539f131cf07c 53 pwm_A.period(1.0/2500.0);
Socrates 26:539f131cf07c 54 pwm_B.period(1.0/2500.0);
Socrates 0:b13a317308d1 55 Ticker looptimer;
Socrates 15:ab236d7c32d2 56 Timeout dirtimeout;
Socrates 27:5d0c94b991aa 57 const double ts=0.004;
Socrates 1:34202d107458 58 looptimer.attach(setlooptimerflag,ts);
Socrates 27:5d0c94b991aa 59 double numh1,numh2,denh2,numl1,numl2,numl3,denl2,denl3;
Socrates 27:5d0c94b991aa 60 double xtr,ytr,y1tr,x1tr,ztr,z1tr,z2tr,yabstr,yabs1tr,yabs2tr,ktr;
Socrates 27:5d0c94b991aa 61 double xbr,ybr,y1br,x1br,zbr,z1br,z2br,yabsbr,yabs1br,yabs2br,kbr;
Socrates 27:5d0c94b991aa 62 double xtl,ytl,y1tl,x1tl,ztl,z1tl,z2tl,yabstl,yabs1tl,yabs2tl,ktl;
Socrates 27:5d0c94b991aa 63 double xbl,ybl,y1bl,x1bl,zbl,z1bl,z2bl,yabsbl,yabs1bl,yabs2bl,kbl;
Socrates 27:5d0c94b991aa 64 double zx,zy, xuit,yuit, rt;
Socrates 27:5d0c94b991aa 65 double vxuit, vyuit, Ap, Ad, Ad1, kd, kp, ki, Ai, Ai1, Bp, Bd, Bd1, Bi, Bi1;
Socrates 28:2904487e0a1e 66 double for_A, for_B, ctrlA, ctrlB, kdc, kpc, kic;
Socrates 35:db3385976119 67 double gain, emggrens,frictiona,frictionb, schrijfgainx, schrijfgainy;
Socrates 37:c6d753123173 68 int xdir, ydir, Adir, Bdir, ticka, tickb, refA,refB, errA, errB;
Socrates 37:c6d753123173 69 int Aboven, Aonder, Bboven,Bonder, Astart, Bstart;
Socrates 26:539f131cf07c 70 //Startwaarden.
Socrates 25:bfe7c49e76cd 71 x1tr=0;
Socrates 25:bfe7c49e76cd 72 y1tr=0;
Socrates 25:bfe7c49e76cd 73 z1tr=0;
Socrates 25:bfe7c49e76cd 74 z2tr=0;
Socrates 25:bfe7c49e76cd 75 yabs1tr=0;
Socrates 25:bfe7c49e76cd 76 yabs2tr=0;
Socrates 25:bfe7c49e76cd 77 x1br=0;
Socrates 25:bfe7c49e76cd 78 y1br=0;
Socrates 25:bfe7c49e76cd 79 z1br=0;
Socrates 25:bfe7c49e76cd 80 z2br=0;
Socrates 25:bfe7c49e76cd 81 yabs1br=0;
Socrates 25:bfe7c49e76cd 82 yabs2br=0;
Socrates 25:bfe7c49e76cd 83 x1tl=0;
Socrates 25:bfe7c49e76cd 84 y1tl=0;
Socrates 25:bfe7c49e76cd 85 z1tl=0;
Socrates 25:bfe7c49e76cd 86 z2tl=0;
Socrates 25:bfe7c49e76cd 87 yabs1tl=0;
Socrates 25:bfe7c49e76cd 88 yabs2tl=0;
Socrates 25:bfe7c49e76cd 89 x1bl=0;
Socrates 25:bfe7c49e76cd 90 y1bl=0;
Socrates 25:bfe7c49e76cd 91 z1bl=0;
Socrates 25:bfe7c49e76cd 92 z2bl=0;
Socrates 25:bfe7c49e76cd 93 yabs1bl=0;
Socrates 25:bfe7c49e76cd 94 yabs2bl=0;
Socrates 25:bfe7c49e76cd 95 zx=0;
Socrates 25:bfe7c49e76cd 96 zy=0;
Socrates 25:bfe7c49e76cd 97 xdir=0;
Socrates 25:bfe7c49e76cd 98 ydir=0;
Socrates 25:bfe7c49e76cd 99 xuit=0;
Socrates 25:bfe7c49e76cd 100 yuit=0;
Socrates 28:2904487e0a1e 101 kpc=1.0*0.1545;
Socrates 28:2904487e0a1e 102 kdc=0.0*2.8*pow(10.0,-3.0);
Socrates 28:2904487e0a1e 103 kic=0.1*1.0;
Socrates 32:5ae627e1bce8 104 kp=1*0.1545;
Socrates 32:5ae627e1bce8 105 kd=1.0*2.8*pow(10.0,-3.0);
Socrates 36:3fad1225c3ad 106 ki=0.05*1.0;
Socrates 37:c6d753123173 107 rt=0.032805; //Straal tandwiel.
Socrates 37:c6d753123173 108 gain=4.0; //Tegen filterverlies.
Socrates 29:8600b02ab223 109 emggrens=0.35;
Socrates 36:3fad1225c3ad 110 frictiona=0.65;
Socrates 35:db3385976119 111 frictionb=0.5;
Socrates 35:db3385976119 112 schrijfgainx=0.1;
Socrates 35:db3385976119 113 schrijfgainy=0.05;
Socrates 37:c6d753123173 114 Astart=0;
Socrates 37:c6d753123173 115 Bstart=1200; //Beginpositie pen in ticks.
Socrates 25:bfe7c49e76cd 116 Ai1=0;
Socrates 25:bfe7c49e76cd 117 Ad1=0;
Socrates 25:bfe7c49e76cd 118 Bi1=0;
Socrates 25:bfe7c49e76cd 119 Bd1=0;
Socrates 26:539f131cf07c 120 pc.baud(115200);
Socrates 29:8600b02ab223 121 Aboven=820;
Socrates 29:8600b02ab223 122 Aonder=165;
Socrates 29:8600b02ab223 123 Bboven=10900;
Socrates 37:c6d753123173 124 Bonder=3400; //Grenzen in ticks. Komen overeen met hoeken van A4.
Socrates 18:6c0200364678 125
Socrates 26:539f131cf07c 126 //Filtercoëfficienten.
Socrates 26:539f131cf07c 127 //High pass, 35Hz, 1e orde, 4 ms.
Socrates 25:bfe7c49e76cd 128 numh1=0.680011076547878;
Socrates 25:bfe7c49e76cd 129 numh2=-0.680011076547878;
Socrates 2:3dab90d3aac2 130 //denh1=1;
Socrates 25:bfe7c49e76cd 131 denh2=-0.360022153095757;
Socrates 25:bfe7c49e76cd 132
Socrates 26:539f131cf07c 133 //Low pass, 5 Hz, 2e orde, 4 ms.
Socrates 25:bfe7c49e76cd 134 numl1=0.003621681514929;
Socrates 25:bfe7c49e76cd 135 numl2=0.007243363029857;
Socrates 25:bfe7c49e76cd 136 numl3=0.003621681514929;
Socrates 2:3dab90d3aac2 137 //denl1=1;
Socrates 25:bfe7c49e76cd 138 denl2=-1.822694925196308;
Socrates 25:bfe7c49e76cd 139 denl3=0.837181651256023;
Socrates 27:5d0c94b991aa 140
Socrates 35:db3385976119 141 //Calibratie
Socrates 30:c569058f10aa 142 wait(4);
Socrates 28:2904487e0a1e 143 while(calflag==true) {
Socrates 35:db3385976119 144 motor1.setPosition(Astart);
Socrates 35:db3385976119 145 motor2.setPosition(Bstart);
Socrates 28:2904487e0a1e 146 while(looptimerflag != true);
Socrates 28:2904487e0a1e 147 looptimerflag = false;
Socrates 28:2904487e0a1e 148 refA=515;
Socrates 37:c6d753123173 149 refB=3536; //515 - 3536 voor rechtsonder.
Socrates 28:2904487e0a1e 150 while(calB==true) {
Socrates 28:2904487e0a1e 151 tickb=motor2.getPosition();
Socrates 28:2904487e0a1e 152 errB=refB-tickb;
Socrates 28:2904487e0a1e 153 Bp=errB*kpc;
Socrates 28:2904487e0a1e 154 Bd=(errB-Bd1)*kdc/ts;
Socrates 28:2904487e0a1e 155 Bi=(Bi1+ts*errB)*kic;
Socrates 28:2904487e0a1e 156 Bd1=Bd;
Socrates 28:2904487e0a1e 157 Bi1=Bi;
Socrates 28:2904487e0a1e 158 ctrlB=(Bi+Bp+Bd);
Socrates 28:2904487e0a1e 159 for_B=(ctrlB)/1000.0;
Socrates 30:c569058f10aa 160 if(ctrlB<0.0) {
Socrates 28:2904487e0a1e 161 Bdir=0;
Socrates 28:2904487e0a1e 162 } else {
Socrates 28:2904487e0a1e 163 Bdir=1;
Socrates 28:2904487e0a1e 164 }
Socrates 30:c569058f10aa 165 keep_in_range(&for_B, -1.0,1.0);
Socrates 28:2904487e0a1e 166 if (frictionflag==true) {
Socrates 35:db3385976119 167 for_B=abs(for_B)+frictionb;
Socrates 37:c6d753123173 168 keep_in_range(&for_B, 0.0,0.1);
Socrates 37:c6d753123173 169 //Vreemd dat een bovengrens van 0.1 werkt ook al is friction groter dan 0.1. Bij hogere waarden gaat de motor te snel.
Socrates 28:2904487e0a1e 170 }
Socrates 28:2904487e0a1e 171 pwm_B.write(abs(for_B));
Socrates 28:2904487e0a1e 172 motordirB.write(Bdir);
Socrates 37:c6d753123173 173 if(errB<20) {
Socrates 28:2904487e0a1e 174 calB=false;
Socrates 30:c569058f10aa 175 pwm_B.write(0.0);
Socrates 28:2904487e0a1e 176 }
Socrates 27:5d0c94b991aa 177 }
Socrates 26:539f131cf07c 178
Socrates 28:2904487e0a1e 179 while(calA==true) {
Socrates 37:c6d753123173 180 ticka=-1*motor1.getPosition(); //Omdat de motor op zijn kop staat.
Socrates 28:2904487e0a1e 181 errA=refA-ticka;
Socrates 28:2904487e0a1e 182 Ap=errA*kpc;
Socrates 28:2904487e0a1e 183 Ad=(errA-Ad1)*kdc/ts;
Socrates 28:2904487e0a1e 184 Ai=(Ai1+ts*errA)*kic;
Socrates 28:2904487e0a1e 185 Ad1=Ad;
Socrates 28:2904487e0a1e 186 Ai1=Ai;
Socrates 28:2904487e0a1e 187 ctrlA=(Ai+Ap+Ad);
Socrates 28:2904487e0a1e 188 for_A=(ctrlA)/1000.0;
Socrates 33:c495e9d8ea1f 189 if(ctrlA<0) {
Socrates 28:2904487e0a1e 190 Adir=1;
Socrates 28:2904487e0a1e 191 } else {
Socrates 28:2904487e0a1e 192 Adir=0;
Socrates 28:2904487e0a1e 193 }
Socrates 28:2904487e0a1e 194 keep_in_range(&for_A, -1,1);
Socrates 28:2904487e0a1e 195 if (frictionflag==true) {
Socrates 35:db3385976119 196 for_A=abs(for_A)+frictiona;
Socrates 33:c495e9d8ea1f 197 keep_in_range(&for_A, 0,0.1);
Socrates 28:2904487e0a1e 198 }
Socrates 28:2904487e0a1e 199 pwm_A.write(abs(for_A));
Socrates 28:2904487e0a1e 200 motordirA.write(Adir);
Socrates 28:2904487e0a1e 201 if(errA<20) {
Socrates 28:2904487e0a1e 202 calA=false;
Socrates 28:2904487e0a1e 203 pwm_A.write(0);
Socrates 28:2904487e0a1e 204 calflag=false;
Socrates 28:2904487e0a1e 205 }
Socrates 27:5d0c94b991aa 206 }
Socrates 27:5d0c94b991aa 207 }
Socrates 26:539f131cf07c 208
Socrates 37:c6d753123173 209 //Meetloop.
Socrates 29:8600b02ab223 210 wait(1);
Socrates 36:3fad1225c3ad 211 //motor1.setPosition(515);
Socrates 36:3fad1225c3ad 212 //motor2.setPosition(3565);
Socrates 37:c6d753123173 213 //Bovenstaande toegevoegd voor het geval de calibratie handmatig gedaan wordt.
Socrates 28:2904487e0a1e 214 while(meetflag==true) {
Socrates 27:5d0c94b991aa 215 while(looptimerflag != true);
Socrates 0:b13a317308d1 216 looptimerflag = false;
Socrates 19:09c4b5249cec 217 //EMG lezen.
Socrates 37:c6d753123173 218 //tr = triceps rechts.
Socrates 25:bfe7c49e76cd 219 ktr=emgtr.read();
Socrates 8:43cce9f7a006 220 xtr=(ktr-0.5)*2.0;
Socrates 8:43cce9f7a006 221 ytr=xtr*numh1+x1tr*numh2-y1tr*denh2;
Socrates 8:43cce9f7a006 222 yabstr=abs(ytr);
Socrates 8:43cce9f7a006 223 ztr=yabstr*numl1+yabs1tr*numl2+yabs2tr*numl3-z1tr*denl2-z2tr*denl3;
Socrates 25:bfe7c49e76cd 224 x1tr=xtr;
Socrates 25:bfe7c49e76cd 225 y1tr=ytr;
Socrates 25:bfe7c49e76cd 226 z2tr=z1tr;
Socrates 25:bfe7c49e76cd 227 z1tr=ztr;
Socrates 25:bfe7c49e76cd 228 yabs2tr=yabs1tr;
Socrates 25:bfe7c49e76cd 229 yabs1tr=yabstr;
Socrates 1:34202d107458 230
Socrates 25:bfe7c49e76cd 231 kbr=emgbr.read();
Socrates 8:43cce9f7a006 232 xbr=(kbr-0.5)*2.0;
Socrates 8:43cce9f7a006 233 ybr=xbr*numh1+x1br*numh2-y1br*denh2;
Socrates 8:43cce9f7a006 234 yabsbr=abs(ybr);
Socrates 8:43cce9f7a006 235 zbr=yabsbr*numl1+yabs1br*numl2+yabs2br*numl3-z1br*denl2-z2br*denl3;
Socrates 25:bfe7c49e76cd 236 x1br=xbr;
Socrates 25:bfe7c49e76cd 237 y1br=ybr;
Socrates 25:bfe7c49e76cd 238 z2br=z1br;
Socrates 25:bfe7c49e76cd 239 z1br=zbr;
Socrates 25:bfe7c49e76cd 240 yabs2br=yabs1br;
Socrates 25:bfe7c49e76cd 241 yabs1br=yabsbr;
Socrates 6:27a4e8d9ddac 242
Socrates 25:bfe7c49e76cd 243 ktl=emgtl.read();
Socrates 8:43cce9f7a006 244 xtl=(ktl-0.5)*2.0;
Socrates 8:43cce9f7a006 245 ytl=xtl*numh1+x1tl*numh2-y1tl*denh2;
Socrates 8:43cce9f7a006 246 yabstl=abs(ytl);
Socrates 8:43cce9f7a006 247 ztl=yabstl*numl1+yabs1tl*numl2+yabs2tl*numl3-z1tl*denl2-z2tl*denl3;
Socrates 25:bfe7c49e76cd 248 x1tl=xtl;
Socrates 25:bfe7c49e76cd 249 y1tl=ytl;
Socrates 25:bfe7c49e76cd 250 z2tl=z1tl;
Socrates 25:bfe7c49e76cd 251 z1tl=ztl;
Socrates 25:bfe7c49e76cd 252 yabs2tl=yabs1tl;
Socrates 25:bfe7c49e76cd 253 yabs1tl=yabstl;
Socrates 5:1a1ce2f5cb66 254
Socrates 25:bfe7c49e76cd 255 kbl=emgbl.read();
Socrates 8:43cce9f7a006 256 xbl=(kbl-0.5)*2.0;
Socrates 8:43cce9f7a006 257 ybl=xbl*numh1+x1bl*numh2-y1bl*denh2;
Socrates 8:43cce9f7a006 258 yabsbl=abs(ybl);
Socrates 8:43cce9f7a006 259 zbl=yabsbl*numl1+yabs1bl*numl2+yabs2bl*numl3-z1bl*denl2-z2bl*denl3;
Socrates 25:bfe7c49e76cd 260 x1bl=xbl;
Socrates 25:bfe7c49e76cd 261 y1bl=ybl;
Socrates 25:bfe7c49e76cd 262 z2bl=z1bl;
Socrates 25:bfe7c49e76cd 263 z1bl=zbl;
Socrates 25:bfe7c49e76cd 264 yabs2bl=yabs1bl;
Socrates 25:bfe7c49e76cd 265 yabs1bl=yabsbl;
Socrates 19:09c4b5249cec 266
Socrates 26:539f131cf07c 267 //Gains om filter te compenseren.
Socrates 34:961aec0a13c6 268 zx=(zbr*gain);
Socrates 26:539f131cf07c 269 zy=(zbl*gain);
Socrates 25:bfe7c49e76cd 270
Socrates 37:c6d753123173 271 //Grenzen en threshold voor emg.
Socrates 8:43cce9f7a006 272 if (zx>1.0) {
Socrates 9:006f34c9f750 273 zx=0.99999;
Socrates 8:43cce9f7a006 274 }
Socrates 8:43cce9f7a006 275 if (zy>1.0) {
Socrates 9:006f34c9f750 276 zy=0.99999;
Socrates 8:43cce9f7a006 277 }
Socrates 28:2904487e0a1e 278 if (zx<emggrens) {
Socrates 28:2904487e0a1e 279 zx=emggrens;
Socrates 17:7dd6dc3c7902 280 }
Socrates 28:2904487e0a1e 281 if (zy<emggrens) {
Socrates 28:2904487e0a1e 282 zy=emggrens;
Socrates 9:006f34c9f750 283 }
Socrates 37:c6d753123173 284 //Schaling om ook outputs lager dan de grenswaarde te krijgen.
Socrates 28:2904487e0a1e 285 zx=zx-emggrens;
Socrates 37:c6d753123173 286 zx=zx/(1.0-emggrens);
Socrates 28:2904487e0a1e 287 zy=zy-emggrens;
Socrates 37:c6d753123173 288 zy=zy/(1.0-emggrens);
Socrates 26:539f131cf07c 289
Socrates 26:539f131cf07c 290 //Richting omdraaien met triceps.
Socrates 32:5ae627e1bce8 291 if (ztr>0.15 && dirflagx == true) {
Socrates 17:7dd6dc3c7902 292 dirflagx = false;
Socrates 15:ab236d7c32d2 293 xdir ^= 1;
Socrates 24:2e5290d8230b 294 dirtimeout.attach(tricheck,1.0);
Socrates 15:ab236d7c32d2 295 }
Socrates 32:5ae627e1bce8 296 if (ztl>0.15 && dirflagy == true) {
Socrates 17:7dd6dc3c7902 297 dirflagy = false;
Socrates 17:7dd6dc3c7902 298 ydir ^= 1;
Socrates 24:2e5290d8230b 299 dirtimeout.attach(tricheck,1.0);
Socrates 17:7dd6dc3c7902 300 }
Socrates 25:bfe7c49e76cd 301
Socrates 37:c6d753123173 302 //Bepalen juiste teken van verplaatsing.
Socrates 29:8600b02ab223 303 if (ydir==0) {
Socrates 25:bfe7c49e76cd 304 zy=-1.0*zy;
Socrates 33:c495e9d8ea1f 305 redled.write(1);
Socrates 19:09c4b5249cec 306 }
Socrates 34:961aec0a13c6 307 if (ydir==1) {
Socrates 34:961aec0a13c6 308 redled.write(0);
Socrates 33:c495e9d8ea1f 309 }
Socrates 33:c495e9d8ea1f 310 if (xdir==0) {
Socrates 25:bfe7c49e76cd 311 zx=-1.0*zx;
Socrates 33:c495e9d8ea1f 312 greenled.write(1);
Socrates 33:c495e9d8ea1f 313 }
Socrates 33:c495e9d8ea1f 314 if (xdir==1) {
Socrates 34:961aec0a13c6 315 greenled.write(0);
Socrates 19:09c4b5249cec 316 }
Socrates 25:bfe7c49e76cd 317
Socrates 37:c6d753123173 318 ticka=-1*motor1.getPosition(); //Zie calibratie.
Socrates 25:bfe7c49e76cd 319 tickb=motor2.getPosition();
Socrates 28:2904487e0a1e 320
Socrates 35:db3385976119 321 //Omzetten naar schrijfsnelheid.
Socrates 35:db3385976119 322 vxuit=zx*schrijfgainx;
Socrates 35:db3385976119 323 vyuit=zy*schrijfgainy;
Socrates 19:09c4b5249cec 324 xuit += ts*vxuit;
Socrates 37:c6d753123173 325 yuit += ts*vyuit; //Integreren naar positie.
Socrates 25:bfe7c49e76cd 326
Socrates 37:c6d753123173 327 //Begrenzing penpositie, getallen in meter.
Socrates 33:c495e9d8ea1f 328 keep_in_range(&xuit,0.115,0.422);
Socrates 33:c495e9d8ea1f 329 keep_in_range(&yuit,0.115,0.335);
Socrates 29:8600b02ab223 330
Socrates 37:c6d753123173 331 //Inversie kinematica en begrenzing motorhoeken.
Socrates 26:539f131cf07c 332 refA=4123.0*atan2(yuit,xuit)/(2.0*PI);
Socrates 26:539f131cf07c 333 refB=4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt);
Socrates 29:8600b02ab223 334 keep_in_rangeint(&refA,Aonder,Aboven);
Socrates 29:8600b02ab223 335 keep_in_rangeint(&refB,Bonder,Bboven);
Socrates 27:5d0c94b991aa 336
Socrates 37:c6d753123173 337 //Controllers
Socrates 20:013e9c00e058 338 errA=refA-ticka;
Socrates 25:bfe7c49e76cd 339 errB=refB-tickb;
Socrates 25:bfe7c49e76cd 340 Ap=errA*kp;
Socrates 25:bfe7c49e76cd 341 Ad=(errA-Ad1)*kd/ts;
Socrates 25:bfe7c49e76cd 342 Ai=(Ai1+ts*errA)*ki;
Socrates 25:bfe7c49e76cd 343 Ad1=Ad;
Socrates 25:bfe7c49e76cd 344 Ai1=Ai;
Socrates 22:a658a3e7b3f4 345 ctrlA=(Ai+Ap+Ad);
Socrates 34:961aec0a13c6 346 for_A=(ctrlA)/10.0;
Socrates 25:bfe7c49e76cd 347
Socrates 25:bfe7c49e76cd 348 Bp=errB*kp;
Socrates 25:bfe7c49e76cd 349 Bd=(errB-Bd1)*kd/ts;
Socrates 25:bfe7c49e76cd 350 Bi=(Bi1+ts*errB)*ki;
Socrates 25:bfe7c49e76cd 351 Bd1=Bd;
Socrates 25:bfe7c49e76cd 352 Bi1=Bi;
Socrates 22:a658a3e7b3f4 353 ctrlB=(Bi+Bp+Bd);
Socrates 33:c495e9d8ea1f 354 for_B=(ctrlB)/1500.0;
Socrates 25:bfe7c49e76cd 355
Socrates 37:c6d753123173 356 //Motorrichting bepalen.
Socrates 34:961aec0a13c6 357 if(ctrlA<0) {
Socrates 29:8600b02ab223 358 Adir=1;
Socrates 25:bfe7c49e76cd 359 } else {
Socrates 29:8600b02ab223 360 Adir=0;
Socrates 25:bfe7c49e76cd 361 }
Socrates 25:bfe7c49e76cd 362 if(ctrlB<0) {
Socrates 25:bfe7c49e76cd 363 Bdir=0;
Socrates 25:bfe7c49e76cd 364 } else {
Socrates 27:5d0c94b991aa 365 Bdir=1;
Socrates 25:bfe7c49e76cd 366 }
Socrates 26:539f131cf07c 367
Socrates 37:c6d753123173 368 //PWM-begrenzing.
Socrates 28:2904487e0a1e 369 keep_in_range(&for_A, -1.0,1.0);
Socrates 28:2904487e0a1e 370 keep_in_range(&for_B, -1.0,1.0);
Socrates 28:2904487e0a1e 371
Socrates 37:c6d753123173 372 //Wrijvingscompensatie.
Socrates 28:2904487e0a1e 373 if (frictionflag==true) {
Socrates 35:db3385976119 374 for_A=abs(for_A)+frictiona;
Socrates 35:db3385976119 375 for_B=abs(for_B)+frictionb;
Socrates 35:db3385976119 376 keep_in_range(&for_A, frictiona,1);
Socrates 35:db3385976119 377 keep_in_range(&for_B, frictionb,1);
Socrates 35:db3385976119 378 }
Socrates 28:2904487e0a1e 379
Socrates 20:013e9c00e058 380 motordirA.write(Adir);
Socrates 20:013e9c00e058 381 motordirB.write(Bdir);
Socrates 22:a658a3e7b3f4 382 pwm_A.write(abs(for_A));
Socrates 22:a658a3e7b3f4 383 pwm_B.write(abs(for_B));
Socrates 25:bfe7c49e76cd 384
Socrates 37:c6d753123173 385
Socrates 37:c6d753123173 386 //Verscheidene prints die nuttig zijn gebeleken.
Socrates 27:5d0c94b991aa 387 if(pc.txBufferGetSize(0)-pc.txBufferGetCount() > 100) {
Socrates 29:8600b02ab223 388 //pc.printf(" %f %f \n\r",zx, zy);
Socrates 33:c495e9d8ea1f 389 pc.printf(" %f %f %f %f\n\r",zbl,ztl,zbr,ztr);
Socrates 29:8600b02ab223 390 //pc.printf(" %f %i %i %f %i %i\n\r",for_A,ticka,refA,for_B,tickb, refB);
Socrates 33:c495e9d8ea1f 391 //pc.printf(" %i %i %i %i %f %f \n\r", ticka,tickb,refA,refB,xuit*1000.0,yuit*1000.0);
Socrates 33:c495e9d8ea1f 392 //pc.printf("A %f B %f\n\r",ctrlA,ctrlB);
Socrates 26:539f131cf07c 393 //pc.printf(" %f %f %f %f \n\r",kbl,zy,vyuit,yuit);
Socrates 26:539f131cf07c 394 //pc.printf(" %i %i %i %f\n\r",tickb,refB,errB,for_B);
Socrates 26:539f131cf07c 395 //pc.printf(" %f %f %f %f \n\r",kbl,zy,vyuit, yuit);
Socrates 26:539f131cf07c 396 //pc.printf("A %i %i %i %i %f %f\n\r",ticka,refA,errA,ctrlA,for_A);
Socrates 28:2904487e0a1e 397 //pc.printf("tr %f br %f tl %f bl %f\n\r",ztr,zbr,ztl,zbl);
Socrates 25:bfe7c49e76cd 398 //pc.printf("%i\n",motor2.getPosition());
Socrates 27:5d0c94b991aa 399 //pc.printf("%f %f\n\r",for_A,for_B);
Socrates 27:5d0c94b991aa 400 }
Socrates 0:b13a317308d1 401 }
Socrates 18:6c0200364678 402 }
Socrates 18:6c0200364678 403
Socrates 27:5d0c94b991aa 404 void keep_in_range(double * in, double min, double max)
Socrates 27:5d0c94b991aa 405 {
Socrates 27:5d0c94b991aa 406 *in > min ? *in < max? : *in = max: *in = min;
Socrates 27:5d0c94b991aa 407 }
Socrates 27:5d0c94b991aa 408 void keep_in_rangeint(int * in, int min, int max)
Socrates 18:6c0200364678 409 {
Socrates 25:bfe7c49e76cd 410 *in > min ? *in < max? : *in = max: *in = min;
Socrates 0:b13a317308d1 411 }