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@31:5c90e931dbfe, 2013-11-04 (annotated)

Committer:
Socrates
Date:
Mon Nov 04 08:15:19 2013 +0000
Revision:
31:5c90e931dbfe
Parent:
30:c569058f10aa
Child:
32:5ae627e1bce8
Voor Notes proberen

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 20:013e9c00e058 5 //XenY
Socrates 25:bfe7c49e76cd 6 //Een pwm van 0.05 is net genoeg om de heugel te bewegen. linksom bewegen is negatief voor de encoder. getposition gaat in ticks.
Socrates 25:bfe7c49e76cd 7 //4123 ticks is een rondje.
Socrates 20:013e9c00e058 8 //Rechts is x, links is y
Socrates 26:539f131cf07c 9
Socrates 26:539f131cf07c 10 //Inputs.
Socrates 8:43cce9f7a006 11 AnalogIn emgtr(PTB3);
Socrates 8:43cce9f7a006 12 AnalogIn emgbr(PTB2);
Socrates 8:43cce9f7a006 13 AnalogIn emgtl(PTB1);
Socrates 8:43cce9f7a006 14 AnalogIn emgbl(PTB0);
Socrates 20:013e9c00e058 15 PwmOut pwm_A(PTA12);
Socrates 20:013e9c00e058 16 PwmOut pwm_B(PTA5);
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 30:c569058f10aa 40 volatile bool startflag=true;
Socrates 30:c569058f10aa 41 volatile bool calA=true, calB=true;
Socrates 30:c569058f10aa 42 volatile bool frictionflag=true;
Socrates 30:c569058f10aa 43
Socrates 28:2904487e0a1e 44 volatile bool calflag=true;
Socrates 28:2904487e0a1e 45 volatile bool meetflag=true;
Socrates 28:2904487e0a1e 46 volatile bool patroonflag=false;
Socrates 27:5d0c94b991aa 47
Socrates 0:b13a317308d1 48 int main()
Socrates 0:b13a317308d1 49 {
Socrates 26:539f131cf07c 50 //Constantes en tickers.
Socrates 26:539f131cf07c 51 pwm_A.period(1.0/2500.0);
Socrates 26:539f131cf07c 52 pwm_B.period(1.0/2500.0);
Socrates 0:b13a317308d1 53 Ticker looptimer;
Socrates 15:ab236d7c32d2 54 Timeout dirtimeout;
Socrates 27:5d0c94b991aa 55 const double ts=0.004;
Socrates 1:34202d107458 56 looptimer.attach(setlooptimerflag,ts);
Socrates 27:5d0c94b991aa 57 double numh1,numh2,denh2,numl1,numl2,numl3,denl2,denl3;
Socrates 27:5d0c94b991aa 58 double xtr,ytr,y1tr,x1tr,ztr,z1tr,z2tr,yabstr,yabs1tr,yabs2tr,ktr;
Socrates 27:5d0c94b991aa 59 double xbr,ybr,y1br,x1br,zbr,z1br,z2br,yabsbr,yabs1br,yabs2br,kbr;
Socrates 27:5d0c94b991aa 60 double xtl,ytl,y1tl,x1tl,ztl,z1tl,z2tl,yabstl,yabs1tl,yabs2tl,ktl;
Socrates 27:5d0c94b991aa 61 double xbl,ybl,y1bl,x1bl,zbl,z1bl,z2bl,yabsbl,yabs1bl,yabs2bl,kbl;
Socrates 27:5d0c94b991aa 62 double zx,zy, xuit,yuit, rt;
Socrates 27:5d0c94b991aa 63 double vxuit, vyuit, Ap, Ad, Ad1, kd, kp, ki, Ai, Ai1, Bp, Bd, Bd1, Bi, Bi1;
Socrates 28:2904487e0a1e 64 double for_A, for_B, ctrlA, ctrlB, kdc, kpc, kic;
Socrates 28:2904487e0a1e 65 double gain, emggrens,friction;
Socrates 28:2904487e0a1e 66 int xdir, ydir, Adir, Bdir;
Socrates 20:013e9c00e058 67 int ticka, tickb, refA,refB, errA, errB;
Socrates 27:5d0c94b991aa 68 int Aboven, Aonder, Bboven,Bonder;
Socrates 26:539f131cf07c 69 //Startwaarden.
Socrates 25:bfe7c49e76cd 70 x1tr=0;
Socrates 25:bfe7c49e76cd 71 y1tr=0;
Socrates 25:bfe7c49e76cd 72 z1tr=0;
Socrates 25:bfe7c49e76cd 73 z2tr=0;
Socrates 25:bfe7c49e76cd 74 yabs1tr=0;
Socrates 25:bfe7c49e76cd 75 yabs2tr=0;
Socrates 25:bfe7c49e76cd 76 x1br=0;
Socrates 25:bfe7c49e76cd 77 y1br=0;
Socrates 25:bfe7c49e76cd 78 z1br=0;
Socrates 25:bfe7c49e76cd 79 z2br=0;
Socrates 25:bfe7c49e76cd 80 yabs1br=0;
Socrates 25:bfe7c49e76cd 81 yabs2br=0;
Socrates 25:bfe7c49e76cd 82 x1tl=0;
Socrates 25:bfe7c49e76cd 83 y1tl=0;
Socrates 25:bfe7c49e76cd 84 z1tl=0;
Socrates 25:bfe7c49e76cd 85 z2tl=0;
Socrates 25:bfe7c49e76cd 86 yabs1tl=0;
Socrates 25:bfe7c49e76cd 87 yabs2tl=0;
Socrates 25:bfe7c49e76cd 88 x1bl=0;
Socrates 25:bfe7c49e76cd 89 y1bl=0;
Socrates 25:bfe7c49e76cd 90 z1bl=0;
Socrates 25:bfe7c49e76cd 91 z2bl=0;
Socrates 25:bfe7c49e76cd 92 yabs1bl=0;
Socrates 25:bfe7c49e76cd 93 yabs2bl=0;
Socrates 25:bfe7c49e76cd 94 zx=0;
Socrates 25:bfe7c49e76cd 95 zy=0;
Socrates 25:bfe7c49e76cd 96 xdir=0;
Socrates 25:bfe7c49e76cd 97 ydir=0;
Socrates 25:bfe7c49e76cd 98 xuit=0;
Socrates 25:bfe7c49e76cd 99 yuit=0;
Socrates 28:2904487e0a1e 100 kpc=1.0*0.1545;
Socrates 28:2904487e0a1e 101 kdc=0.0*2.8*pow(10.0,-3.0);
Socrates 28:2904487e0a1e 102 kic=0.1*1.0;
Socrates 28:2904487e0a1e 103 kp=0.1*0.1545;
Socrates 28:2904487e0a1e 104 kd=0.1*2.8*pow(10.0,-3.0);
Socrates 26:539f131cf07c 105 ki=0.1*1.0;
Socrates 25:bfe7c49e76cd 106 rt=0.032805;
Socrates 29:8600b02ab223 107 gain=4.0;
Socrates 29:8600b02ab223 108 emggrens=0.35;
Socrates 28:2904487e0a1e 109 friction=0.4;
Socrates 25:bfe7c49e76cd 110 Ai1=0;
Socrates 25:bfe7c49e76cd 111 Ad1=0;
Socrates 25:bfe7c49e76cd 112 Bi1=0;
Socrates 25:bfe7c49e76cd 113 Bd1=0;
Socrates 26:539f131cf07c 114 pc.baud(115200);
Socrates 29:8600b02ab223 115 Aboven=820;
Socrates 29:8600b02ab223 116 Aonder=165;
Socrates 29:8600b02ab223 117 Bboven=10900;
Socrates 29:8600b02ab223 118 Bonder=3400;
Socrates 18:6c0200364678 119
Socrates 26:539f131cf07c 120 //Filtercoëfficienten.
Socrates 26:539f131cf07c 121 //High pass, 35Hz, 1e orde, 4 ms.
Socrates 25:bfe7c49e76cd 122 numh1=0.680011076547878;
Socrates 25:bfe7c49e76cd 123 numh2=-0.680011076547878;
Socrates 2:3dab90d3aac2 124 //denh1=1;
Socrates 25:bfe7c49e76cd 125 denh2=-0.360022153095757;
Socrates 25:bfe7c49e76cd 126
Socrates 26:539f131cf07c 127 //Low pass, 5 Hz, 2e orde, 4 ms.
Socrates 25:bfe7c49e76cd 128 numl1=0.003621681514929;
Socrates 25:bfe7c49e76cd 129 numl2=0.007243363029857;
Socrates 25:bfe7c49e76cd 130 numl3=0.003621681514929;
Socrates 2:3dab90d3aac2 131 //denl1=1;
Socrates 25:bfe7c49e76cd 132 denl2=-1.822694925196308;
Socrates 25:bfe7c49e76cd 133 denl3=0.837181651256023;
Socrates 26:539f131cf07c 134 //Low pass, 2 Hz, 2e orde, 1 ms.
Socrates 24:2e5290d8230b 135 //numl1=0.391302053991682*pow(10.0,-4.0);
Socrates 24:2e5290d8230b 136 //numl2=0.782604107983365*pow(10.0,-4.0);
Socrates 24:2e5290d8230b 137 //numl3=0.391302053991682*pow(10.0,-4.0);
Socrates 24:2e5290d8230b 138 //denl1=1;
Socrates 24:2e5290d8230b 139 //denl2=-1.982228929792529;
Socrates 24:2e5290d8230b 140 //denl3=0.982385450614126;
Socrates 27:5d0c94b991aa 141
Socrates 26:539f131cf07c 142 //Opzetje voor calibratie
Socrates 30:c569058f10aa 143 wait(4);
Socrates 26:539f131cf07c 144
Socrates 30:c569058f10aa 145 if(startflag==true);
Socrates 30:c569058f10aa 146 {
Socrates 30:c569058f10aa 147 motor1.setPosition(0);
Socrates 30:c569058f10aa 148 motor2.setPosition(1200);
Socrates 30:c569058f10aa 149 startflag=false;
Socrates 29:8600b02ab223 150 }
Socrates 30:c569058f10aa 151
Socrates 28:2904487e0a1e 152 while(calflag==true) {
Socrates 28:2904487e0a1e 153 while(looptimerflag != true);
Socrates 28:2904487e0a1e 154 looptimerflag = false;
Socrates 30:c569058f10aa 155 //515 - 3536 voor rechtsonder.
Socrates 28:2904487e0a1e 156 refA=515;
Socrates 28:2904487e0a1e 157 refB=3536;
Socrates 28:2904487e0a1e 158 while(calB==true) {
Socrates 28:2904487e0a1e 159 tickb=motor2.getPosition();
Socrates 28:2904487e0a1e 160 errB=refB-tickb;
Socrates 28:2904487e0a1e 161 Bp=errB*kpc;
Socrates 28:2904487e0a1e 162 Bd=(errB-Bd1)*kdc/ts;
Socrates 28:2904487e0a1e 163 Bi=(Bi1+ts*errB)*kic;
Socrates 28:2904487e0a1e 164 Bd1=Bd;
Socrates 28:2904487e0a1e 165 Bi1=Bi;
Socrates 28:2904487e0a1e 166 ctrlB=(Bi+Bp+Bd);
Socrates 28:2904487e0a1e 167 for_B=(ctrlB)/1000.0;
Socrates 30:c569058f10aa 168 if(ctrlB<0.0) {
Socrates 28:2904487e0a1e 169 Bdir=0;
Socrates 28:2904487e0a1e 170 } else {
Socrates 28:2904487e0a1e 171 Bdir=1;
Socrates 28:2904487e0a1e 172 }
Socrates 30:c569058f10aa 173 keep_in_range(&for_B, -1.0,1.0);
Socrates 28:2904487e0a1e 174 if (frictionflag==true) {
Socrates 28:2904487e0a1e 175 for_B=abs(for_B)+friction;
Socrates 30:c569058f10aa 176 keep_in_range(&for_B, 0.0,0.1);
Socrates 28:2904487e0a1e 177 }
Socrates 28:2904487e0a1e 178 pwm_B.write(abs(for_B));
Socrates 28:2904487e0a1e 179 motordirB.write(Bdir);
Socrates 28:2904487e0a1e 180 //pc.printf("B %f %i \n\r",for_B,errB);
Socrates 28:2904487e0a1e 181 if(errB<10) {
Socrates 28:2904487e0a1e 182 calB=false;
Socrates 30:c569058f10aa 183 pwm_B.write(0.0);
Socrates 28:2904487e0a1e 184 }
Socrates 27:5d0c94b991aa 185 }
Socrates 26:539f131cf07c 186
Socrates 28:2904487e0a1e 187 while(calA==true) {
Socrates 30:c569058f10aa 188 ticka=-motor1.getPosition(); //NOTE: deze moet volgens mij gewoon weer positief zijn.
Socrates 28:2904487e0a1e 189 errA=refA-ticka;
Socrates 28:2904487e0a1e 190 Ap=errA*kpc;
Socrates 28:2904487e0a1e 191 Ad=(errA-Ad1)*kdc/ts;
Socrates 28:2904487e0a1e 192 Ai=(Ai1+ts*errA)*kic;
Socrates 28:2904487e0a1e 193 Ad1=Ad;
Socrates 28:2904487e0a1e 194 Ai1=Ai;
Socrates 28:2904487e0a1e 195 ctrlA=(Ai+Ap+Ad);
Socrates 28:2904487e0a1e 196 for_A=(ctrlA)/1000.0;
Socrates 30:c569058f10aa 197 if(ctrlA<0) { //NOTE: als ticka weer +get is, kan deze weer 0 else 1 zijn.
Socrates 28:2904487e0a1e 198 Adir=1;
Socrates 28:2904487e0a1e 199 } else {
Socrates 28:2904487e0a1e 200 Adir=0;
Socrates 28:2904487e0a1e 201 }
Socrates 28:2904487e0a1e 202 keep_in_range(&for_A, -1,1);
Socrates 28:2904487e0a1e 203 if (frictionflag==true) {
Socrates 28:2904487e0a1e 204 for_A=abs(for_A)+friction;
Socrates 28:2904487e0a1e 205 keep_in_range(&for_A, 0,0.08);
Socrates 28:2904487e0a1e 206 }
Socrates 28:2904487e0a1e 207 pwm_A.write(abs(for_A));
Socrates 28:2904487e0a1e 208 motordirA.write(Adir);
Socrates 28:2904487e0a1e 209 //pc.printf("A %f %i \n\r",for_A,errA);
Socrates 28:2904487e0a1e 210 if(errA<20) {
Socrates 28:2904487e0a1e 211 calA=false;
Socrates 28:2904487e0a1e 212 pwm_A.write(0);
Socrates 28:2904487e0a1e 213 calflag=false;
Socrates 28:2904487e0a1e 214 }
Socrates 27:5d0c94b991aa 215 }
Socrates 27:5d0c94b991aa 216 }
Socrates 26:539f131cf07c 217 //Einde opzetje.
Socrates 26:539f131cf07c 218
Socrates 26:539f131cf07c 219 //Loop.
Socrates 29:8600b02ab223 220 wait(1);
Socrates 28:2904487e0a1e 221 while(meetflag==true) {
Socrates 27:5d0c94b991aa 222 while(looptimerflag != true);
Socrates 25:bfe7c49e76cd 223 {
Socrates 13:e0e9fda0e9a1 224 }
Socrates 0:b13a317308d1 225 looptimerflag = false;
Socrates 19:09c4b5249cec 226 //EMG lezen.
Socrates 25:bfe7c49e76cd 227 ktr=emgtr.read();
Socrates 8:43cce9f7a006 228 xtr=(ktr-0.5)*2.0;
Socrates 8:43cce9f7a006 229 ytr=xtr*numh1+x1tr*numh2-y1tr*denh2;
Socrates 8:43cce9f7a006 230 yabstr=abs(ytr);
Socrates 8:43cce9f7a006 231 ztr=yabstr*numl1+yabs1tr*numl2+yabs2tr*numl3-z1tr*denl2-z2tr*denl3;
Socrates 25:bfe7c49e76cd 232 x1tr=xtr;
Socrates 25:bfe7c49e76cd 233 y1tr=ytr;
Socrates 25:bfe7c49e76cd 234 z2tr=z1tr;
Socrates 25:bfe7c49e76cd 235 z1tr=ztr;
Socrates 25:bfe7c49e76cd 236 yabs2tr=yabs1tr;
Socrates 25:bfe7c49e76cd 237 yabs1tr=yabstr;
Socrates 1:34202d107458 238
Socrates 25:bfe7c49e76cd 239 kbr=emgbr.read();
Socrates 8:43cce9f7a006 240 xbr=(kbr-0.5)*2.0;
Socrates 8:43cce9f7a006 241 ybr=xbr*numh1+x1br*numh2-y1br*denh2;
Socrates 8:43cce9f7a006 242 yabsbr=abs(ybr);
Socrates 8:43cce9f7a006 243 zbr=yabsbr*numl1+yabs1br*numl2+yabs2br*numl3-z1br*denl2-z2br*denl3;
Socrates 25:bfe7c49e76cd 244 x1br=xbr;
Socrates 25:bfe7c49e76cd 245 y1br=ybr;
Socrates 25:bfe7c49e76cd 246 z2br=z1br;
Socrates 25:bfe7c49e76cd 247 z1br=zbr;
Socrates 25:bfe7c49e76cd 248 yabs2br=yabs1br;
Socrates 25:bfe7c49e76cd 249 yabs1br=yabsbr;
Socrates 6:27a4e8d9ddac 250
Socrates 25:bfe7c49e76cd 251 ktl=emgtl.read();
Socrates 8:43cce9f7a006 252 xtl=(ktl-0.5)*2.0;
Socrates 8:43cce9f7a006 253 ytl=xtl*numh1+x1tl*numh2-y1tl*denh2;
Socrates 8:43cce9f7a006 254 yabstl=abs(ytl);
Socrates 8:43cce9f7a006 255 ztl=yabstl*numl1+yabs1tl*numl2+yabs2tl*numl3-z1tl*denl2-z2tl*denl3;
Socrates 25:bfe7c49e76cd 256 x1tl=xtl;
Socrates 25:bfe7c49e76cd 257 y1tl=ytl;
Socrates 25:bfe7c49e76cd 258 z2tl=z1tl;
Socrates 25:bfe7c49e76cd 259 z1tl=ztl;
Socrates 25:bfe7c49e76cd 260 yabs2tl=yabs1tl;
Socrates 25:bfe7c49e76cd 261 yabs1tl=yabstl;
Socrates 5:1a1ce2f5cb66 262
Socrates 25:bfe7c49e76cd 263 kbl=emgbl.read();
Socrates 8:43cce9f7a006 264 xbl=(kbl-0.5)*2.0;
Socrates 8:43cce9f7a006 265 ybl=xbl*numh1+x1bl*numh2-y1bl*denh2;
Socrates 8:43cce9f7a006 266 yabsbl=abs(ybl);
Socrates 8:43cce9f7a006 267 zbl=yabsbl*numl1+yabs1bl*numl2+yabs2bl*numl3-z1bl*denl2-z2bl*denl3;
Socrates 25:bfe7c49e76cd 268 x1bl=xbl;
Socrates 25:bfe7c49e76cd 269 y1bl=ybl;
Socrates 25:bfe7c49e76cd 270 z2bl=z1bl;
Socrates 25:bfe7c49e76cd 271 z1bl=zbl;
Socrates 25:bfe7c49e76cd 272 yabs2bl=yabs1bl;
Socrates 25:bfe7c49e76cd 273 yabs1bl=yabsbl;
Socrates 19:09c4b5249cec 274
Socrates 26:539f131cf07c 275 //Gains om filter te compenseren.
Socrates 26:539f131cf07c 276 zx=(zbr*gain);
Socrates 26:539f131cf07c 277 zy=(zbl*gain);
Socrates 25:bfe7c49e76cd 278
Socrates 28:2904487e0a1e 279 //Grenzen voor emg.
Socrates 8:43cce9f7a006 280 if (zx>1.0) {
Socrates 9:006f34c9f750 281 zx=0.99999;
Socrates 8:43cce9f7a006 282 }
Socrates 8:43cce9f7a006 283 if (zy>1.0) {
Socrates 9:006f34c9f750 284 zy=0.99999;
Socrates 8:43cce9f7a006 285 }
Socrates 28:2904487e0a1e 286 if (zx<emggrens) {
Socrates 28:2904487e0a1e 287 zx=emggrens;
Socrates 17:7dd6dc3c7902 288 }
Socrates 28:2904487e0a1e 289 if (zy<emggrens) {
Socrates 28:2904487e0a1e 290 zy=emggrens;
Socrates 9:006f34c9f750 291 }
Socrates 28:2904487e0a1e 292 zx=zx-emggrens;
Socrates 28:2904487e0a1e 293 zx=zx*(1.0/(1.0-emggrens));
Socrates 28:2904487e0a1e 294 zy=zy-emggrens;
Socrates 28:2904487e0a1e 295 zy=zy*(1.0/(1.0-emggrens));
Socrates 26:539f131cf07c 296
Socrates 26:539f131cf07c 297 //Richting omdraaien met triceps.
Socrates 31:5c90e931dbfe 298 if (ztr>0.1 && dirflagx == true) {
Socrates 17:7dd6dc3c7902 299 dirflagx = false;
Socrates 15:ab236d7c32d2 300 xdir ^= 1;
Socrates 30:c569058f10aa 301 //zx=0; //NOTE: deze weghalen kan schelen?
Socrates 24:2e5290d8230b 302 dirtimeout.attach(tricheck,1.0);
Socrates 15:ab236d7c32d2 303 }
Socrates 31:5c90e931dbfe 304 if (ztl>0.1 && dirflagy == true) {
Socrates 17:7dd6dc3c7902 305 dirflagy = false;
Socrates 17:7dd6dc3c7902 306 ydir ^= 1;
Socrates 30:c569058f10aa 307 //zy=0;
Socrates 24:2e5290d8230b 308 dirtimeout.attach(tricheck,1.0);
Socrates 17:7dd6dc3c7902 309 }
Socrates 25:bfe7c49e76cd 310
Socrates 19:09c4b5249cec 311 //Motoraansturing.
Socrates 29:8600b02ab223 312 if (ydir==0) {
Socrates 25:bfe7c49e76cd 313 zy=-1.0*zy;
Socrates 19:09c4b5249cec 314 }
Socrates 30:c569058f10aa 315 if (xdir==1) { //NOTE: moet dit geen 0 zijn?
Socrates 25:bfe7c49e76cd 316 zx=-1.0*zx;
Socrates 19:09c4b5249cec 317 }
Socrates 25:bfe7c49e76cd 318
Socrates 30:c569058f10aa 319 ticka=-1*motor1.getPosition(); //NOTE: -1 weghalen?
Socrates 25:bfe7c49e76cd 320 tickb=motor2.getPosition();
Socrates 28:2904487e0a1e 321
Socrates 27:5d0c94b991aa 322 //Begrenzing.
Socrates 30:c569058f10aa 323 vxuit=zx*4.0*pow(10.0,-1.0); // 4cm/s //NOTE: deze shizzle kan er misschien uit. In ieder geval de vier. En anders moet het /1000 zijn.
Socrates 29:8600b02ab223 324 vyuit=zy*4.0*pow(10.0,-2.0); // 4cm/s
Socrates 19:09c4b5249cec 325 xuit += ts*vxuit;
Socrates 20:013e9c00e058 326 yuit += ts*vyuit;
Socrates 25:bfe7c49e76cd 327
Socrates 30:c569058f10aa 328 keep_in_range(&xuit,-4.5,2.75); //NOTE: Iets met de grenzen. En de richtingpins.
Socrates 30:c569058f10aa 329 keep_in_range(&yuit,-4.5,2.8); //NOTE: als bovenstaande weer klopt moeten de grenzen voor x zijn: .125 en 0.422. Voor y is dat 0.125 en 0.335.
Socrates 29:8600b02ab223 330
Socrates 26:539f131cf07c 331 refA=4123.0*atan2(yuit,xuit)/(2.0*PI);
Socrates 26:539f131cf07c 332 refB=4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt);
Socrates 29:8600b02ab223 333 keep_in_rangeint(&refA,Aonder,Aboven);
Socrates 29:8600b02ab223 334 keep_in_rangeint(&refB,Bonder,Bboven);
Socrates 27:5d0c94b991aa 335
Socrates 20:013e9c00e058 336 errA=refA-ticka;
Socrates 25:bfe7c49e76cd 337 errB=refB-tickb;
Socrates 25:bfe7c49e76cd 338
Socrates 21:659f7c8ed328 339 //Controllers
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 23:ee89be59ae2f 343 //keep_in_range(&Ad,-0.1,0.1);
Socrates 23:ee89be59ae2f 344 //keep_in_range(&Ai,-0.1,0.1);
Socrates 25:bfe7c49e76cd 345 Ad1=Ad;
Socrates 25:bfe7c49e76cd 346 Ai1=Ai;
Socrates 22:a658a3e7b3f4 347 ctrlA=(Ai+Ap+Ad);
Socrates 30:c569058f10aa 348 for_A=(ctrlA)/700.0; //NOTE: maak eens een plot van ctrlA en ctrlB. Kan leerzaam zijn voor deze gains.
Socrates 25:bfe7c49e76cd 349
Socrates 25:bfe7c49e76cd 350 Bp=errB*kp;
Socrates 25:bfe7c49e76cd 351 Bd=(errB-Bd1)*kd/ts;
Socrates 25:bfe7c49e76cd 352 Bi=(Bi1+ts*errB)*ki;
Socrates 23:ee89be59ae2f 353 //keep_in_range(&Bd,-0.1,0.1);
Socrates 23:ee89be59ae2f 354 //keep_in_range(&Bi,-0.1,0.1);
Socrates 25:bfe7c49e76cd 355 Bd1=Bd;
Socrates 25:bfe7c49e76cd 356 Bi1=Bi;
Socrates 22:a658a3e7b3f4 357 ctrlB=(Bi+Bp+Bd);
Socrates 28:2904487e0a1e 358 for_B=(ctrlB)/7000.0;
Socrates 25:bfe7c49e76cd 359 //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks.
Socrates 25:bfe7c49e76cd 360
Socrates 30:c569058f10aa 361 if(ctrlA<0) { //NOTE: zeker gezien de vorige omdraaiingen moet deze volgens mij weer 0 else 1 zijn.
Socrates 29:8600b02ab223 362 Adir=1;
Socrates 25:bfe7c49e76cd 363 } else {
Socrates 29:8600b02ab223 364 Adir=0;
Socrates 25:bfe7c49e76cd 365 }
Socrates 25:bfe7c49e76cd 366 if(ctrlB<0) {
Socrates 25:bfe7c49e76cd 367 Bdir=0;
Socrates 25:bfe7c49e76cd 368 } else {
Socrates 27:5d0c94b991aa 369 Bdir=1;
Socrates 25:bfe7c49e76cd 370 }
Socrates 26:539f131cf07c 371
Socrates 28:2904487e0a1e 372 keep_in_range(&for_A, -1.0,1.0);
Socrates 28:2904487e0a1e 373 keep_in_range(&for_B, -1.0,1.0);
Socrates 28:2904487e0a1e 374
Socrates 28:2904487e0a1e 375 if (frictionflag==true) {
Socrates 28:2904487e0a1e 376 for_A=abs(for_A)+friction;
Socrates 28:2904487e0a1e 377 for_B=abs(for_B)+friction;
Socrates 28:2904487e0a1e 378
Socrates 30:c569058f10aa 379 keep_in_range(&for_A, 0,1.0); //NOTE: misschien valt hier nog wat te begrenzen, maar dan wel op ondergrens friction.
Socrates 29:8600b02ab223 380 keep_in_range(&for_B, 0,1.0);
Socrates 28:2904487e0a1e 381 }
Socrates 20:013e9c00e058 382 motordirA.write(Adir);
Socrates 20:013e9c00e058 383 motordirB.write(Bdir);
Socrates 22:a658a3e7b3f4 384 pwm_A.write(abs(for_A));
Socrates 22:a658a3e7b3f4 385 pwm_B.write(abs(for_B));
Socrates 25:bfe7c49e76cd 386
Socrates 27:5d0c94b991aa 387 if(pc.txBufferGetSize(0)-pc.txBufferGetCount() > 100) {
Socrates 29:8600b02ab223 388 //pc.printf(" %f %f \n\r",zx, zy);
Socrates 29:8600b02ab223 389 //pc.printf(" %f %i %i %f %i %i\n\r",for_A,ticka,refA,for_B,tickb, refB);
Socrates 29:8600b02ab223 390 pc.printf(" %i %i %i %i %f %f \n\r", ticka,tickb,refA,refB,xuit,yuit);
Socrates 27:5d0c94b991aa 391 //pc.printf("A %i %i B %i %i\n\r",Adir, ticka,Bdir, tickb);
Socrates 26:539f131cf07c 392 //pc.printf(" %f %f %f %f \n\r",kbl,zy,vyuit,yuit);
Socrates 26:539f131cf07c 393 //pc.printf(" %i %i %i %f\n\r",tickb,refB,errB,for_B);
Socrates 26:539f131cf07c 394 //pc.printf(" %f %f %f %f \n\r",kbl,zy,vyuit, yuit);
Socrates 26:539f131cf07c 395 //pc.printf("A %i %i %i %i %f %f\n\r",ticka,refA,errA,ctrlA,for_A);
Socrates 28:2904487e0a1e 396 //pc.printf("tr %f br %f tl %f bl %f\n\r",ztr,zbr,ztl,zbl);
Socrates 25:bfe7c49e76cd 397 //pc.printf("%i\n",motor2.getPosition());
Socrates 27:5d0c94b991aa 398 //pc.printf("%f %f\n\r",for_A,for_B);
Socrates 27:5d0c94b991aa 399 }
Socrates 0:b13a317308d1 400 }
Socrates 28:2904487e0a1e 401
Socrates 28:2904487e0a1e 402 while(patroonflag==true) {
Socrates 29:8600b02ab223 403 while(looptimerflag != true);
Socrates 29:8600b02ab223 404 looptimerflag = false;
Socrates 29:8600b02ab223 405 xuit=400.0;
Socrates 29:8600b02ab223 406 yuit=140.0;
Socrates 29:8600b02ab223 407 ticka=-1*motor1.getPosition();
Socrates 29:8600b02ab223 408 tickb=motor2.getPosition();
Socrates 29:8600b02ab223 409 refA=4123.0*atan2(yuit,xuit)/(2.0*PI);
Socrates 29:8600b02ab223 410 refB=4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt*1000.0);
Socrates 29:8600b02ab223 411 errA=refA-ticka;
Socrates 29:8600b02ab223 412 errB=refB-tickb;
Socrates 29:8600b02ab223 413 while(errA>20 && errB>20) {
Socrates 28:2904487e0a1e 414 ticka=-1*motor1.getPosition();
Socrates 28:2904487e0a1e 415 tickb=motor2.getPosition();
Socrates 28:2904487e0a1e 416 refA=4123.0*atan2(yuit,xuit)/(2.0*PI);
Socrates 28:2904487e0a1e 417 refB=4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt);
Socrates 28:2904487e0a1e 418 errA=refA-ticka;
Socrates 28:2904487e0a1e 419 errB=refB-tickb;
Socrates 28:2904487e0a1e 420 //Controllers
Socrates 29:8600b02ab223 421 Ap=errA*kpc;
Socrates 29:8600b02ab223 422 Ad=(errA-Ad1)*kdc/ts;
Socrates 29:8600b02ab223 423 Ai=(Ai1+ts*errA)*kic;
Socrates 28:2904487e0a1e 424 Ad1=Ad;
Socrates 28:2904487e0a1e 425 Ai1=Ai;
Socrates 28:2904487e0a1e 426 ctrlA=(Ai+Ap+Ad);
Socrates 29:8600b02ab223 427 for_A=(ctrlA)/1000.0;
Socrates 29:8600b02ab223 428 Bp=errB*kpc;
Socrates 29:8600b02ab223 429 Bd=(errB-Bd1)*kdc/ts;
Socrates 29:8600b02ab223 430 Bi=(Bi1+ts*errB)*kic;
Socrates 28:2904487e0a1e 431 Bd1=Bd;
Socrates 28:2904487e0a1e 432 Bi1=Bi;
Socrates 28:2904487e0a1e 433 ctrlB=(Bi+Bp+Bd);
Socrates 29:8600b02ab223 434 for_B=(ctrlB)/1000.0;
Socrates 28:2904487e0a1e 435 if(ctrlA<0) {
Socrates 28:2904487e0a1e 436 Adir=0;
Socrates 28:2904487e0a1e 437 } else {
Socrates 28:2904487e0a1e 438 Adir=1;
Socrates 28:2904487e0a1e 439 }
Socrates 28:2904487e0a1e 440 if(ctrlB<0) {
Socrates 28:2904487e0a1e 441 Bdir=0;
Socrates 28:2904487e0a1e 442 } else {
Socrates 28:2904487e0a1e 443 Bdir=1;
Socrates 28:2904487e0a1e 444 }
Socrates 28:2904487e0a1e 445 keep_in_range(&for_A, -1.0,1.0);
Socrates 28:2904487e0a1e 446 keep_in_range(&for_B, -1.0,1.0);
Socrates 28:2904487e0a1e 447 if (frictionflag==true) {
Socrates 28:2904487e0a1e 448 for_A=abs(for_A)+friction;
Socrates 28:2904487e0a1e 449 for_B=abs(for_B)+friction;
Socrates 29:8600b02ab223 450 keep_in_range(&for_A, 0,0.08);
Socrates 29:8600b02ab223 451 keep_in_range(&for_B, 0,0.1);
Socrates 28:2904487e0a1e 452 }
Socrates 28:2904487e0a1e 453 motordirA.write(Adir);
Socrates 28:2904487e0a1e 454 motordirB.write(Bdir);
Socrates 28:2904487e0a1e 455 pwm_A.write(abs(for_A));
Socrates 28:2904487e0a1e 456 pwm_B.write(abs(for_B));
Socrates 29:8600b02ab223 457 }
Socrates 29:8600b02ab223 458 wait(1);
Socrates 29:8600b02ab223 459 xuit=400.0;
Socrates 29:8600b02ab223 460 yuit=180.0;
Socrates 29:8600b02ab223 461 while(errA>20 && errB>20) {
Socrates 29:8600b02ab223 462 ticka=-1*motor1.getPosition();
Socrates 29:8600b02ab223 463 tickb=motor2.getPosition();
Socrates 29:8600b02ab223 464 refA=4123.0*atan2(yuit,xuit)/(2.0*PI);
Socrates 29:8600b02ab223 465 refB=4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt*1000.0);
Socrates 29:8600b02ab223 466 //keep_in_rangeint(&refA,Aonder,Aboven);
Socrates 29:8600b02ab223 467 //keep_in_rangeint(&refB,Bonder,Bboven);
Socrates 29:8600b02ab223 468 errA=refA-ticka;
Socrates 29:8600b02ab223 469 errB=refB-tickb;
Socrates 29:8600b02ab223 470 //Controllers
Socrates 29:8600b02ab223 471 Ap=errA*kpc;
Socrates 29:8600b02ab223 472 Ad=(errA-Ad1)*kdc/ts;
Socrates 29:8600b02ab223 473 Ai=(Ai1+ts*errA)*kic;
Socrates 29:8600b02ab223 474 Ad1=Ad;
Socrates 29:8600b02ab223 475 Ai1=Ai;
Socrates 29:8600b02ab223 476 ctrlA=(Ai+Ap+Ad);
Socrates 29:8600b02ab223 477 for_A=(ctrlA)/1000.0;
Socrates 29:8600b02ab223 478 Bp=errB*kpc;
Socrates 29:8600b02ab223 479 Bd=(errB-Bd1)*kdc/ts;
Socrates 29:8600b02ab223 480 Bi=(Bi1+ts*errB)*kic;
Socrates 29:8600b02ab223 481 Bd1=Bd;
Socrates 29:8600b02ab223 482 Bi1=Bi;
Socrates 29:8600b02ab223 483 ctrlB=(Bi+Bp+Bd);
Socrates 29:8600b02ab223 484 for_B=(ctrlB)/1000.0;
Socrates 29:8600b02ab223 485 if(ctrlA<0) {
Socrates 29:8600b02ab223 486 Adir=0;
Socrates 29:8600b02ab223 487 } else {
Socrates 29:8600b02ab223 488 Adir=1;
Socrates 29:8600b02ab223 489 }
Socrates 29:8600b02ab223 490 if(ctrlB<0) {
Socrates 29:8600b02ab223 491 Bdir=0;
Socrates 29:8600b02ab223 492 } else {
Socrates 29:8600b02ab223 493 Bdir=1;
Socrates 29:8600b02ab223 494 }
Socrates 29:8600b02ab223 495 keep_in_range(&for_A, -1.0,1.0);
Socrates 29:8600b02ab223 496 keep_in_range(&for_B, -1.0,1.0);
Socrates 29:8600b02ab223 497 if (frictionflag==true) {
Socrates 29:8600b02ab223 498 for_A=abs(for_A)+friction;
Socrates 29:8600b02ab223 499 for_B=abs(for_B)+friction;
Socrates 29:8600b02ab223 500 keep_in_range(&for_A, 0,0.08);
Socrates 29:8600b02ab223 501 keep_in_range(&for_B, 0,0.1);
Socrates 29:8600b02ab223 502 }
Socrates 29:8600b02ab223 503 motordirA.write(Adir);
Socrates 29:8600b02ab223 504 motordirB.write(Bdir);
Socrates 29:8600b02ab223 505 pwm_A.write(abs(for_A));
Socrates 29:8600b02ab223 506 pwm_B.write(abs(for_B));
Socrates 28:2904487e0a1e 507 }
Socrates 28:2904487e0a1e 508 }
Socrates 28:2904487e0a1e 509
Socrates 18:6c0200364678 510 }
Socrates 18:6c0200364678 511
Socrates 27:5d0c94b991aa 512 void keep_in_range(double * in, double min, double max)
Socrates 27:5d0c94b991aa 513 {
Socrates 27:5d0c94b991aa 514 *in > min ? *in < max? : *in = max: *in = min;
Socrates 27:5d0c94b991aa 515 }
Socrates 27:5d0c94b991aa 516 void keep_in_rangeint(int * in, int min, int max)
Socrates 18:6c0200364678 517 {
Socrates 25:bfe7c49e76cd 518 *in > min ? *in < max? : *in = max: *in = min;
Socrates 0:b13a317308d1 519 }