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@20:013e9c00e058, 2013-10-29 (annotated)

Committer:
Socrates
Date:
Tue Oct 29 11:11:19 2013 +0000
Revision:
20:013e9c00e058
Parent:
19:09c4b5249cec
Child:
21:659f7c8ed328
90 %;

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