Thomas Plaisier / Mbed 2 deprecated G3_Barry_Plotter.

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MODSERIAL mbed Encoder

main.cpp@37:c6d753123173, 2013-11-07 (annotated)

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