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@23:ee89be59ae2f, 2013-10-29 (annotated)

Committer:
Socrates
Date:
Tue Oct 29 14:41:05 2013 +0000
Revision:
23:ee89be59ae2f
Parent:
22:a658a3e7b3f4
Child:
24:2e5290d8230b
Code werkt grotendeels, maar rare sprongen bij encoder.

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 22:a658a3e7b3f4 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 22:a658a3e7b3f4 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 22:a658a3e7b3f4 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 22:a658a3e7b3f4 34 dirflagx=true;
Socrates 22:a658a3e7b3f4 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 22:a658a3e7b3f4 52 float xuit,yuit, rt;
Socrates 20:013e9c00e058 53 int xdir, ydir, Adir, Bdir;
Socrates 19:09c4b5249cec 54 float vxuit, vyuit, Ap, Ad, Ad1, kd, kp, ki, Ai, Ai1, Bp, Bd, Bd1, Bi, Bi1;
Socrates 22:a658a3e7b3f4 55 float for_A, for_B;
Socrates 20:013e9c00e058 56 int ticka, tickb, refA,refB, errA, errB;
Socrates 20:013e9c00e058 57 float ctrlA;
Socrates 20:013e9c00e058 58 float ctrlB;
Socrates 20:013e9c00e058 59 //Startwaarden
Socrates 22:a658a3e7b3f4 60 x1tr=0; y1tr=0; z1tr=0; z2tr=0; yabs1tr=0; yabs2tr=0;
Socrates 22:a658a3e7b3f4 61 x1br=0; y1br=0; z1br=0; z2br=0; yabs1br=0; yabs2br=0;
Socrates 22:a658a3e7b3f4 62 x1tl=0; y1tl=0; z1tl=0; z2tl=0; yabs1tl=0; yabs2tl=0;
Socrates 22:a658a3e7b3f4 63 x1bl=0; y1bl=0; z1bl=0; z2bl=0; yabs1bl=0; yabs2bl=0;
Socrates 22:a658a3e7b3f4 64 zx=0; zy=0; xdir=0; ydir=0; xuit=0; yuit=0;
Socrates 23:ee89be59ae2f 65 kp=0.1; ki=0.0001; kd=0.0001; rt=0.032805; Ai1=0; Ad1=0;
Socrates 22:a658a3e7b3f4 66 Bi1=0; Bd1=0;
Socrates 18:6c0200364678 67
Socrates 20:013e9c00e058 68 //High pass, 35Hz, 1e orde
Socrates 6:27a4e8d9ddac 69 numh1=0.900575535279376;
Socrates 6:27a4e8d9ddac 70 numh2=-0.900575535279376;
Socrates 2:3dab90d3aac2 71 //denh1=1;
Socrates 6:27a4e8d9ddac 72 denh2=-0.801151070558751;
Socrates 22:a658a3e7b3f4 73
Socrates 18:6c0200364678 74 //Low pass, 5 Hz, 2e orde.
Socrates 18:6c0200364678 75 numl1=0.241359049041961*pow(10.0,-3.0);
Socrates 18:6c0200364678 76 numl2=0.482718098083923*pow(10.0,-3.0);
Socrates 18:6c0200364678 77 numl3=0.241359049041961*pow(10.0,-3.0);
Socrates 2:3dab90d3aac2 78 //denl1=1;
Socrates 18:6c0200364678 79 denl2=-1.955578240315036;
Socrates 18:6c0200364678 80 denl3=0.956543676511203;
Socrates 0:b13a317308d1 81 pc.baud(115200);
Socrates 0:b13a317308d1 82
Socrates 0:b13a317308d1 83 while(1) {
Socrates 22:a658a3e7b3f4 84 while(looptimerflag != true);{
Socrates 13:e0e9fda0e9a1 85 }
Socrates 0:b13a317308d1 86 looptimerflag = false;
Socrates 6:27a4e8d9ddac 87
Socrates 19:09c4b5249cec 88 //EMG lezen.
Socrates 22:a658a3e7b3f4 89 ktr=emgtr.read();
Socrates 8:43cce9f7a006 90 xtr=(ktr-0.5)*2.0;
Socrates 8:43cce9f7a006 91 ytr=xtr*numh1+x1tr*numh2-y1tr*denh2;
Socrates 8:43cce9f7a006 92 yabstr=abs(ytr);
Socrates 8:43cce9f7a006 93 ztr=yabstr*numl1+yabs1tr*numl2+yabs2tr*numl3-z1tr*denl2-z2tr*denl3;
Socrates 22:a658a3e7b3f4 94 x1tr=xtr; y1tr=ytr; z2tr=z1tr; z1tr=ztr; yabs2tr=yabs1tr; yabs1tr=yabstr;
Socrates 1:34202d107458 95
Socrates 22:a658a3e7b3f4 96 kbr=emgbr.read();
Socrates 8:43cce9f7a006 97 xbr=(kbr-0.5)*2.0;
Socrates 8:43cce9f7a006 98 ybr=xbr*numh1+x1br*numh2-y1br*denh2;
Socrates 8:43cce9f7a006 99 yabsbr=abs(ybr);
Socrates 8:43cce9f7a006 100 zbr=yabsbr*numl1+yabs1br*numl2+yabs2br*numl3-z1br*denl2-z2br*denl3;
Socrates 22:a658a3e7b3f4 101 x1br=xbr; y1br=ybr; z2br=z1br; z1br=zbr; yabs2br=yabs1br; yabs1br=yabsbr;
Socrates 6:27a4e8d9ddac 102
Socrates 22:a658a3e7b3f4 103 ktl=emgtl.read();
Socrates 8:43cce9f7a006 104 xtl=(ktl-0.5)*2.0;
Socrates 8:43cce9f7a006 105 ytl=xtl*numh1+x1tl*numh2-y1tl*denh2;
Socrates 8:43cce9f7a006 106 yabstl=abs(ytl);
Socrates 8:43cce9f7a006 107 ztl=yabstl*numl1+yabs1tl*numl2+yabs2tl*numl3-z1tl*denl2-z2tl*denl3;
Socrates 22:a658a3e7b3f4 108 x1tl=xtl; y1tl=ytl; z2tl=z1tl; z1tl=ztl; yabs2tl=yabs1tl; yabs1tl=yabstl;
Socrates 5:1a1ce2f5cb66 109
Socrates 22:a658a3e7b3f4 110 kbl=emgbl.read();
Socrates 8:43cce9f7a006 111 xbl=(kbl-0.5)*2.0;
Socrates 8:43cce9f7a006 112 ybl=xbl*numh1+x1bl*numh2-y1bl*denh2;
Socrates 8:43cce9f7a006 113 yabsbl=abs(ybl);
Socrates 8:43cce9f7a006 114 zbl=yabsbl*numl1+yabs1bl*numl2+yabs2bl*numl3-z1bl*denl2-z2bl*denl3;
Socrates 22:a658a3e7b3f4 115 x1bl=xbl; y1bl=ybl; z2bl=z1bl; z1bl=zbl; yabs2bl=yabs1bl; yabs1bl=yabsbl;
Socrates 19:09c4b5249cec 116
Socrates 22:a658a3e7b3f4 117 zx=(zbr*3.0);
Socrates 22:a658a3e7b3f4 118 zy=(zbl*3.0);
Socrates 22:a658a3e7b3f4 119
Socrates 19:09c4b5249cec 120 //Grenzen.
Socrates 8:43cce9f7a006 121 if (zx>1.0) {
Socrates 9:006f34c9f750 122 zx=0.99999;
Socrates 8:43cce9f7a006 123 }
Socrates 8:43cce9f7a006 124 if (zy>1.0) {
Socrates 9:006f34c9f750 125 zy=0.99999;
Socrates 8:43cce9f7a006 126 }
Socrates 22:a658a3e7b3f4 127 if (zx<0.30){
Socrates 22:a658a3e7b3f4 128 zx=0;
Socrates 17:7dd6dc3c7902 129 }
Socrates 22:a658a3e7b3f4 130 if (zy<0.30){
Socrates 22:a658a3e7b3f4 131 zy=0;
Socrates 9:006f34c9f750 132 }
Socrates 22:a658a3e7b3f4 133
Socrates 22:a658a3e7b3f4 134 //Richting omdraaien met triceps.
Socrates 17:7dd6dc3c7902 135 if ((ztr>(zbr+0.1)) && dirflagx == true) {
Socrates 17:7dd6dc3c7902 136 dirflagx = false;
Socrates 15:ab236d7c32d2 137 xdir ^= 1;
Socrates 20:013e9c00e058 138 zx=0;
Socrates 15:ab236d7c32d2 139 dirtimeout.attach(tricheck,1.5);
Socrates 15:ab236d7c32d2 140 }
Socrates 22:a658a3e7b3f4 141 if ((ztl>(zbl+0.1)) && dirflagy == true) {
Socrates 17:7dd6dc3c7902 142 dirflagy = false;
Socrates 17:7dd6dc3c7902 143 ydir ^= 1;
Socrates 20:013e9c00e058 144 zy=0;
Socrates 17:7dd6dc3c7902 145 dirtimeout.attach(tricheck,1.5);
Socrates 17:7dd6dc3c7902 146 }
Socrates 22:a658a3e7b3f4 147
Socrates 19:09c4b5249cec 148 //Motoraansturing.
Socrates 22:a658a3e7b3f4 149 if (ydir==1)
Socrates 22:a658a3e7b3f4 150 {
Socrates 22:a658a3e7b3f4 151 zy=-1.0*zy;
Socrates 19:09c4b5249cec 152 }
Socrates 22:a658a3e7b3f4 153 if (xdir==1)
Socrates 22:a658a3e7b3f4 154 {
Socrates 22:a658a3e7b3f4 155 zx=-1.0*zx;
Socrates 19:09c4b5249cec 156 }
Socrates 22:a658a3e7b3f4 157
Socrates 22:a658a3e7b3f4 158 ticka=motor1.getPosition(); tickb=motor2.getPosition();
Socrates 23:ee89be59ae2f 159 vxuit=zx*1.0*pow(1.0,-2.0); // 4cm/s
Socrates 23:ee89be59ae2f 160 vyuit=zy*1.0*pow(1.0,-2.0); // 4cm/s
Socrates 19:09c4b5249cec 161 xuit += ts*vxuit;
Socrates 20:013e9c00e058 162 yuit += ts*vyuit;
Socrates 22:a658a3e7b3f4 163
Socrates 23:ee89be59ae2f 164 refA=(4123.0*atan2(yuit,xuit)/(2.0*PI));
Socrates 23:ee89be59ae2f 165 refB=(4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt));
Socrates 20:013e9c00e058 166 errA=refA-ticka;
Socrates 22:a658a3e7b3f4 167 errB=refB-tickb;
Socrates 22:a658a3e7b3f4 168
Socrates 22:a658a3e7b3f4 169 if(errA<0){
Socrates 22:a658a3e7b3f4 170 Adir=0;}
Socrates 22:a658a3e7b3f4 171 else{
Socrates 22:a658a3e7b3f4 172 Adir=1;
Socrates 22:a658a3e7b3f4 173 }
Socrates 22:a658a3e7b3f4 174 if(errB<0){
Socrates 22:a658a3e7b3f4 175 Bdir=0;}
Socrates 22:a658a3e7b3f4 176 else{
Socrates 22:a658a3e7b3f4 177 Bdir=1;
Socrates 22:a658a3e7b3f4 178 }
Socrates 21:659f7c8ed328 179 //Controllers
Socrates 22:a658a3e7b3f4 180 Ap=errA*kp; Ad=(errA-Ad1)*kd/ts; Ai=(Ai1+ts*errA)*ki;
Socrates 23:ee89be59ae2f 181 //keep_in_range(&Ad,-0.1,0.1);
Socrates 23:ee89be59ae2f 182 //keep_in_range(&Ai,-0.1,0.1);
Socrates 22:a658a3e7b3f4 183 Ad1=Ad; Ai1=Ai;
Socrates 22:a658a3e7b3f4 184 ctrlA=(Ai+Ap+Ad);
Socrates 23:ee89be59ae2f 185 for_A=(ctrlA)/100.0;
Socrates 19:09c4b5249cec 186
Socrates 22:a658a3e7b3f4 187 Bp=errB*kp; Bd=(errB-Bd1)*kd/ts; Bi=(Bi1+ts*errB)*ki;
Socrates 23:ee89be59ae2f 188 //keep_in_range(&Bd,-0.1,0.1);
Socrates 23:ee89be59ae2f 189 //keep_in_range(&Bi,-0.1,0.1);
Socrates 23:ee89be59ae2f 190
Socrates 22:a658a3e7b3f4 191 Bd1=Bd; Bi1=Bi;
Socrates 22:a658a3e7b3f4 192 ctrlB=(Bi+Bp+Bd);
Socrates 23:ee89be59ae2f 193 for_B=(ctrlB)/100.0;
Socrates 22:a658a3e7b3f4 194 //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks.
Socrates 22:a658a3e7b3f4 195
Socrates 19:09c4b5249cec 196 keep_in_range(&for_A, -1,1);
Socrates 19:09c4b5249cec 197 keep_in_range(&for_B, -1,1);
Socrates 19:09c4b5249cec 198
Socrates 20:013e9c00e058 199 motordirA.write(Adir);
Socrates 20:013e9c00e058 200 motordirB.write(Bdir);
Socrates 22:a658a3e7b3f4 201 pwm_A.write(abs(for_A));
Socrates 22:a658a3e7b3f4 202 pwm_B.write(abs(for_B));
Socrates 22:a658a3e7b3f4 203
Socrates 23:ee89be59ae2f 204 //pc.printf(" %f %f %f %f \n\r",kbl,zy,vyuit, yuit);
Socrates 23:ee89be59ae2f 205 pc.printf(" %i %i %i %f %f\n\r",tickb,refB,errB,ctrlB,for_B);
Socrates 0:b13a317308d1 206 }
Socrates 18:6c0200364678 207 }
Socrates 18:6c0200364678 208
Socrates 18:6c0200364678 209 void keep_in_range(float * in, float min, float max)
Socrates 18:6c0200364678 210 {
Socrates 22:a658a3e7b3f4 211 *in > min ? *in < max? : *in = max: *in = min;
Socrates 0:b13a317308d1 212 }