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@21:659f7c8ed328, 2013-10-29 (annotated)

Committer:
Socrates
Date:
Tue Oct 29 12:17:41 2013 +0000
Revision:
21:659f7c8ed328
Parent:
20:013e9c00e058
Child:
22:a658a3e7b3f4
victor

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 21:659f7c8ed328 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 21:659f7c8ed328 7 //4123 ticks is een rondje.
Socrates 20:013e9c00e058 8 //Rechts is x, links is y
Socrates 8:43cce9f7a006 9 AnalogIn emgtr(PTB3);
Socrates 8:43cce9f7a006 10 AnalogIn emgbr(PTB2);
Socrates 8:43cce9f7a006 11 AnalogIn emgtl(PTB1);
Socrates 8:43cce9f7a006 12 AnalogIn emgbl(PTB0);
Socrates 20:013e9c00e058 13 PwmOut pwm_A(PTA12);
Socrates 20:013e9c00e058 14 PwmOut pwm_B(PTA5);
Socrates 0:b13a317308d1 15 MODSERIAL pc(USBTX,USBRX);
Socrates 17:7dd6dc3c7902 16 DigitalOut motordirA(PTD3);
Socrates 17:7dd6dc3c7902 17 DigitalOut motordirB(PTD1);
Socrates 19:09c4b5249cec 18 Encoder motor1(PTD0,PTC9);
Socrates 19:09c4b5249cec 19 Encoder motor2(PTD5,PTC8);
Socrates 20:013e9c00e058 20
Socrates 18:6c0200364678 21 void keep_in_range(float * in, float min, float max);
Socrates 18:6c0200364678 22
Socrates 0:b13a317308d1 23 volatile bool looptimerflag;
Socrates 0:b13a317308d1 24 void setlooptimerflag(void)
Socrates 0:b13a317308d1 25 {
Socrates 21:659f7c8ed328 26 looptimerflag = true;
Socrates 0:b13a317308d1 27 }
Socrates 0:b13a317308d1 28
Socrates 17:7dd6dc3c7902 29 volatile bool dirflagx=true;
Socrates 17:7dd6dc3c7902 30 volatile bool dirflagy=true;
Socrates 17:7dd6dc3c7902 31
Socrates 13:e0e9fda0e9a1 32 void tricheck(void)
Socrates 13:e0e9fda0e9a1 33 {
Socrates 21:659f7c8ed328 34 dirflagx=true;
Socrates 21:659f7c8ed328 35 dirflagy=true;
Socrates 13:e0e9fda0e9a1 36 }
Socrates 13:e0e9fda0e9a1 37
Socrates 0:b13a317308d1 38 int main()
Socrates 0:b13a317308d1 39 {
Socrates 20:013e9c00e058 40 pwm_A.period(1.0/22000.0);
Socrates 20:013e9c00e058 41 pwm_B.period(1.0/22000.0);
Socrates 0:b13a317308d1 42 Ticker looptimer;
Socrates 15:ab236d7c32d2 43 Timeout dirtimeout;
Socrates 18:6c0200364678 44 const float ts=0.001;
Socrates 1:34202d107458 45 looptimer.attach(setlooptimerflag,ts);
Socrates 18:6c0200364678 46 float numh1,numh2,denh2,numl1,numl2,numl3,denl2,denl3;
Socrates 18:6c0200364678 47 float xtr,ytr,y1tr,x1tr,ztr,z1tr,z2tr,yabstr,yabs1tr,yabs2tr,ktr;
Socrates 18:6c0200364678 48 float xbr,ybr,y1br,x1br,zbr,z1br,z2br,yabsbr,yabs1br,yabs2br,kbr;
Socrates 18:6c0200364678 49 float xtl,ytl,y1tl,x1tl,ztl,z1tl,z2tl,yabstl,yabs1tl,yabs2tl,ktl;
Socrates 18:6c0200364678 50 float xbl,ybl,y1bl,x1bl,zbl,z1bl,z2bl,yabsbl,yabs1bl,yabs2bl,kbl;
Socrates 18:6c0200364678 51 float zx,zy;
Socrates 21:659f7c8ed328 52 float xuit,yuit, rt;
Socrates 20:013e9c00e058 53 //float zx1, zy1, gain;
Socrates 20:013e9c00e058 54 //float pwmA, pwmB;
Socrates 20:013e9c00e058 55 int xdir, ydir, Adir, Bdir;
Socrates 19:09c4b5249cec 56 float vxuit, vyuit, Ap, Ad, Ad1, kd, kp, ki, Ai, Ai1, Bp, Bd, Bd1, Bi, Bi1;
Socrates 20:013e9c00e058 57 float for_A, for_B, yuit1, xuit1;
Socrates 20:013e9c00e058 58 int ticka, tickb, refA,refB, errA, errB;
Socrates 20:013e9c00e058 59 float ctrlA;
Socrates 20:013e9c00e058 60 float ctrlB;
Socrates 20:013e9c00e058 61 //Startwaarden
Socrates 21:659f7c8ed328 62 x1tr=0;
Socrates 21:659f7c8ed328 63 y1tr=0;
Socrates 21:659f7c8ed328 64 z1tr=0;
Socrates 21:659f7c8ed328 65 z2tr=0;
Socrates 21:659f7c8ed328 66 yabs1tr=0;
Socrates 21:659f7c8ed328 67 yabs2tr=0;
Socrates 21:659f7c8ed328 68 x1br=0;
Socrates 21:659f7c8ed328 69 y1br=0;
Socrates 21:659f7c8ed328 70 z1br=0;
Socrates 21:659f7c8ed328 71 z2br=0;
Socrates 21:659f7c8ed328 72 yabs1br=0;
Socrates 21:659f7c8ed328 73 yabs2br=0;
Socrates 21:659f7c8ed328 74 x1tl=0;
Socrates 21:659f7c8ed328 75 y1tl=0;
Socrates 21:659f7c8ed328 76 z1tl=0;
Socrates 21:659f7c8ed328 77 z2tl=0;
Socrates 21:659f7c8ed328 78 yabs1tl=0;
Socrates 21:659f7c8ed328 79 yabs2tl=0;
Socrates 21:659f7c8ed328 80 x1bl=0;
Socrates 21:659f7c8ed328 81 y1bl=0;
Socrates 21:659f7c8ed328 82 z1bl=0;
Socrates 21:659f7c8ed328 83 z2bl=0;
Socrates 21:659f7c8ed328 84 yabs1bl=0;
Socrates 21:659f7c8ed328 85 yabs2bl=0;
Socrates 21:659f7c8ed328 86 zx=0;
Socrates 21:659f7c8ed328 87 zy=0;
Socrates 21:659f7c8ed328 88 xdir=0;
Socrates 21:659f7c8ed328 89 ydir=0;
Socrates 21:659f7c8ed328 90 xuit=0;
Socrates 21:659f7c8ed328 91 yuit=0;
Socrates 21:659f7c8ed328 92 kp=0.1;
Socrates 21:659f7c8ed328 93 ki=0.01;
Socrates 21:659f7c8ed328 94 kd=0.01;
Socrates 21:659f7c8ed328 95 rt=0.032805;
Socrates 21:659f7c8ed328 96 Bi1=0;
Socrates 21:659f7c8ed328 97 Bd1=0;
Socrates 21:659f7c8ed328 98 yuit1=0;
Socrates 21:659f7c8ed328 99 xuit1=0;
Socrates 18:6c0200364678 100
Socrates 20:013e9c00e058 101 //High pass, 35Hz, 1e orde
Socrates 6:27a4e8d9ddac 102 numh1=0.900575535279376;
Socrates 6:27a4e8d9ddac 103 numh2=-0.900575535279376;
Socrates 2:3dab90d3aac2 104 //denh1=1;
Socrates 6:27a4e8d9ddac 105 denh2=-0.801151070558751;
Socrates 21:659f7c8ed328 106
Socrates 18:6c0200364678 107 //Low pass, 5 Hz, 2e orde.
Socrates 18:6c0200364678 108 numl1=0.241359049041961*pow(10.0,-3.0);
Socrates 18:6c0200364678 109 numl2=0.482718098083923*pow(10.0,-3.0);
Socrates 18:6c0200364678 110 numl3=0.241359049041961*pow(10.0,-3.0);
Socrates 2:3dab90d3aac2 111 //denl1=1;
Socrates 18:6c0200364678 112 denl2=-1.955578240315036;
Socrates 18:6c0200364678 113 denl3=0.956543676511203;
Socrates 0:b13a317308d1 114 pc.baud(115200);
Socrates 0:b13a317308d1 115
Socrates 0:b13a317308d1 116 while(1) {
Socrates 21:659f7c8ed328 117 while(looptimerflag != true);
Socrates 21:659f7c8ed328 118 {
Socrates 13:e0e9fda0e9a1 119 }
Socrates 0:b13a317308d1 120 looptimerflag = false;
Socrates 6:27a4e8d9ddac 121
Socrates 19:09c4b5249cec 122 //EMG lezen.
Socrates 21:659f7c8ed328 123 ktr=emgtr.read();
Socrates 8:43cce9f7a006 124 xtr=(ktr-0.5)*2.0;
Socrates 8:43cce9f7a006 125 ytr=xtr*numh1+x1tr*numh2-y1tr*denh2;
Socrates 8:43cce9f7a006 126 yabstr=abs(ytr);
Socrates 8:43cce9f7a006 127 ztr=yabstr*numl1+yabs1tr*numl2+yabs2tr*numl3-z1tr*denl2-z2tr*denl3;
Socrates 21:659f7c8ed328 128 x1tr=xtr;
Socrates 21:659f7c8ed328 129 y1tr=ytr;
Socrates 21:659f7c8ed328 130 z2tr=z1tr;
Socrates 21:659f7c8ed328 131 z1tr=ztr;
Socrates 21:659f7c8ed328 132 yabs2tr=yabs1tr;
Socrates 21:659f7c8ed328 133 yabs1tr=yabstr;
Socrates 1:34202d107458 134
Socrates 21:659f7c8ed328 135 kbr=emgbr.read();
Socrates 8:43cce9f7a006 136 xbr=(kbr-0.5)*2.0;
Socrates 8:43cce9f7a006 137 ybr=xbr*numh1+x1br*numh2-y1br*denh2;
Socrates 8:43cce9f7a006 138 yabsbr=abs(ybr);
Socrates 8:43cce9f7a006 139 zbr=yabsbr*numl1+yabs1br*numl2+yabs2br*numl3-z1br*denl2-z2br*denl3;
Socrates 21:659f7c8ed328 140 x1br=xbr;
Socrates 21:659f7c8ed328 141 y1br=ybr;
Socrates 21:659f7c8ed328 142 z2br=z1br;
Socrates 21:659f7c8ed328 143 z1br=zbr;
Socrates 21:659f7c8ed328 144 yabs2br=yabs1br;
Socrates 21:659f7c8ed328 145 yabs1br=yabsbr;
Socrates 6:27a4e8d9ddac 146
Socrates 21:659f7c8ed328 147 ktl=emgtl.read();
Socrates 8:43cce9f7a006 148 xtl=(ktl-0.5)*2.0;
Socrates 8:43cce9f7a006 149 ytl=xtl*numh1+x1tl*numh2-y1tl*denh2;
Socrates 8:43cce9f7a006 150 yabstl=abs(ytl);
Socrates 8:43cce9f7a006 151 ztl=yabstl*numl1+yabs1tl*numl2+yabs2tl*numl3-z1tl*denl2-z2tl*denl3;
Socrates 21:659f7c8ed328 152 x1tl=xtl;
Socrates 21:659f7c8ed328 153 y1tl=ytl;
Socrates 21:659f7c8ed328 154 z2tl=z1tl;
Socrates 21:659f7c8ed328 155 z1tl=ztl;
Socrates 21:659f7c8ed328 156 yabs2tl=yabs1tl;
Socrates 21:659f7c8ed328 157 yabs1tl=yabstl;
Socrates 5:1a1ce2f5cb66 158
Socrates 21:659f7c8ed328 159 kbl=emgbl.read();
Socrates 8:43cce9f7a006 160 xbl=(kbl-0.5)*2.0;
Socrates 8:43cce9f7a006 161 ybl=xbl*numh1+x1bl*numh2-y1bl*denh2;
Socrates 8:43cce9f7a006 162 yabsbl=abs(ybl);
Socrates 8:43cce9f7a006 163 zbl=yabsbl*numl1+yabs1bl*numl2+yabs2bl*numl3-z1bl*denl2-z2bl*denl3;
Socrates 21:659f7c8ed328 164 x1bl=xbl;
Socrates 21:659f7c8ed328 165 y1bl=ybl;
Socrates 21:659f7c8ed328 166 z2bl=z1bl;
Socrates 21:659f7c8ed328 167 z1bl=zbl;
Socrates 21:659f7c8ed328 168 yabs2bl=yabs1bl;
Socrates 21:659f7c8ed328 169 yabs1bl=yabsbl;
Socrates 19:09c4b5249cec 170
Socrates 20:013e9c00e058 171 zx=(zbr*5.0);
Socrates 20:013e9c00e058 172 zy=(zbl*5.0);
Socrates 21:659f7c8ed328 173
Socrates 19:09c4b5249cec 174 //Grenzen.
Socrates 8:43cce9f7a006 175 if (zx>1.0) {
Socrates 9:006f34c9f750 176 zx=0.99999;
Socrates 8:43cce9f7a006 177 }
Socrates 8:43cce9f7a006 178 if (zy>1.0) {
Socrates 9:006f34c9f750 179 zy=0.99999;
Socrates 8:43cce9f7a006 180 }
Socrates 21:659f7c8ed328 181 if (zx<0.20) {
Socrates 21:659f7c8ed328 182 zx=0;
Socrates 17:7dd6dc3c7902 183 }
Socrates 21:659f7c8ed328 184 if (zy<0.20) {
Socrates 21:659f7c8ed328 185 zy=0;
Socrates 9:006f34c9f750 186 }
Socrates 21:659f7c8ed328 187
Socrates 21:659f7c8ed328 188 //Richting omdraaien met triceps.
Socrates 17:7dd6dc3c7902 189 if ((ztr>(zbr+0.1)) && dirflagx == true) {
Socrates 17:7dd6dc3c7902 190 dirflagx = false;
Socrates 15:ab236d7c32d2 191 xdir ^= 1;
Socrates 20:013e9c00e058 192 zx=0;
Socrates 15:ab236d7c32d2 193 dirtimeout.attach(tricheck,1.5);
Socrates 15:ab236d7c32d2 194 }
Socrates 21:659f7c8ed328 195 if ((ztl>(zbl+0.1)) && dirflagy == true) {
Socrates 17:7dd6dc3c7902 196 dirflagy = false;
Socrates 17:7dd6dc3c7902 197 ydir ^= 1;
Socrates 20:013e9c00e058 198 zy=0;
Socrates 17:7dd6dc3c7902 199 dirtimeout.attach(tricheck,1.5);
Socrates 17:7dd6dc3c7902 200 }
Socrates 21:659f7c8ed328 201
Socrates 19:09c4b5249cec 202 //Motoraansturing.
Socrates 21:659f7c8ed328 203 if (ydir==1) {
Socrates 21:659f7c8ed328 204 zy=-1.0*zy;
Socrates 19:09c4b5249cec 205 }
Socrates 21:659f7c8ed328 206 if (xdir==1) {
Socrates 21:659f7c8ed328 207 zx=-1.0*zx;
Socrates 19:09c4b5249cec 208 }
Socrates 21:659f7c8ed328 209
Socrates 21:659f7c8ed328 210 ticka=motor1.getPosition();
Socrates 21:659f7c8ed328 211 tickb=motor2.getPosition();
Socrates 20:013e9c00e058 212 vxuit=zx*1.0*pow(1.0,-2.0); // 4cm/s
Socrates 20:013e9c00e058 213 vyuit=zy*1.0*pow(1.0,-2.0); // 4cm/s
Socrates 19:09c4b5249cec 214 xuit += ts*vxuit;
Socrates 20:013e9c00e058 215 yuit += ts*vyuit;
Socrates 20:013e9c00e058 216 yuit1=yuit;
Socrates 21:659f7c8ed328 217
Socrates 20:013e9c00e058 218 refA=floor(4123.0*atan2(yuit,xuit)/(2.0*PI));
Socrates 20:013e9c00e058 219 refB=floor(4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt));
Socrates 20:013e9c00e058 220 errA=refA-ticka;
Socrates 21:659f7c8ed328 221 errB=refB-tickb;
Socrates 21:659f7c8ed328 222
Socrates 21:659f7c8ed328 223 if(errA<0) {
Socrates 21:659f7c8ed328 224 Adir=0;
Socrates 21:659f7c8ed328 225 } else {
Socrates 21:659f7c8ed328 226 Adir=1;
Socrates 21:659f7c8ed328 227 }
Socrates 21:659f7c8ed328 228 if(errB<0) {
Socrates 21:659f7c8ed328 229 Bdir=0;
Socrates 21:659f7c8ed328 230 } else {
Socrates 21:659f7c8ed328 231 Bdir=1;
Socrates 21:659f7c8ed328 232 }
Socrates 21:659f7c8ed328 233 //Controllers
Socrates 21:659f7c8ed328 234 Ap=errA*kp;
Socrates 21:659f7c8ed328 235 Ad=(errA-Ad1)*kd/ts;
Socrates 21:659f7c8ed328 236 Ai=(Ai1+ts*errA)*ki;
Socrates 21:659f7c8ed328 237 keep_in_range(&Ai, -0.1,0.1);
Socrates 21:659f7c8ed328 238 Ad1=Ad;
Socrates 21:659f7c8ed328 239 Ai1=Ai;
Socrates 21:659f7c8ed328 240 ctrlA=Ai+Ap+Ad;
Socrates 21:659f7c8ed328 241 for_A = ctrlA/1000.;
Socrates 19:09c4b5249cec 242
Socrates 21:659f7c8ed328 243 errB= 1;
Socrates 21:659f7c8ed328 244 Bp=errB*kp;
Socrates 21:659f7c8ed328 245 Bd=(errB-Bd1)*kd/ts;
Socrates 21:659f7c8ed328 246 Bi=(Bi1+ts*errB)*ki;
Socrates 21:659f7c8ed328 247 keep_in_range(&Bi, -0.1,0.1);
Socrates 21:659f7c8ed328 248 Bd1=Bd;
Socrates 21:659f7c8ed328 249 Bi1=Bi;
Socrates 21:659f7c8ed328 250 ctrlB=Bi+Bp+Bd;
Socrates 21:659f7c8ed328 251 for_B= ctrlB / 1000.0;
Socrates 21:659f7c8ed328 252 //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks.
Socrates 21:659f7c8ed328 253
Socrates 19:09c4b5249cec 254 keep_in_range(&for_A, -1,1);
Socrates 19:09c4b5249cec 255 keep_in_range(&for_B, -1,1);
Socrates 19:09c4b5249cec 256
Socrates 20:013e9c00e058 257 motordirA.write(Adir);
Socrates 20:013e9c00e058 258 motordirB.write(Bdir);
Socrates 19:09c4b5249cec 259 pwm_A.write(for_A);
Socrates 19:09c4b5249cec 260 pwm_B.write(for_B);
Socrates 21:659f7c8ed328 261
Socrates 18:6c0200364678 262 //pc.printf("Bi: %f, Tri: %f, Richting: %d \n\r",zbl*3.0,ztl*3.0,ydir);
Socrates 20:013e9c00e058 263 pc.printf(" %i %i %i %f %f\n\r",tickb,refB,errB,ctrlB,for_B);
Socrates 18:6c0200364678 264 //pc.printf("b %f t %f xu %f yu %f A %f B %f \n\r",zbl,ztl,xuit, yuit, pwmA,pwmB);
Socrates 0:b13a317308d1 265 }
Socrates 18:6c0200364678 266 }
Socrates 18:6c0200364678 267
Socrates 18:6c0200364678 268 void keep_in_range(float * in, float min, float max)
Socrates 18:6c0200364678 269 {
Socrates 21:659f7c8ed328 270 *in > min ? *in < max? : *in = max: *in = min;
Socrates 0:b13a317308d1 271 }