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@16:414a2397c493, 2013-10-25 (annotated)

Committer:
Socrates
Date:
Fri Oct 25 09:44:12 2013 +0000
Revision:
16:414a2397c493
Parent:
14:5746b1697ff4
Parent:
15:ab236d7c32d2
Child:
17:7dd6dc3c7902
merge

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 8:43cce9f7a006 3 AnalogIn emgtr(PTB3);
Socrates 8:43cce9f7a006 4 AnalogIn emgbr(PTB2);
Socrates 9:006f34c9f750 5 //Rechts is x, links is y
Socrates 8:43cce9f7a006 6 AnalogIn emgtl(PTB1);
Socrates 8:43cce9f7a006 7 AnalogIn emgbl(PTB0);
Socrates 11:28a196a3d898 8 PwmOut pwm_x(PTA5);
Socrates 11:28a196a3d898 9 PwmOut pwm_y(PTA12);
Socrates 0:b13a317308d1 10 MODSERIAL pc(USBTX,USBRX);
Socrates 12:100cbba6cd96 11 DigitalOut motordir1(PTD3);
Socrates 12:100cbba6cd96 12 DigitalOut motordir2(PTD1);
Socrates 11:28a196a3d898 13
Socrates 0:b13a317308d1 14 volatile bool looptimerflag;
gerard1993 14:5746b1697ff4 15 //hoi
Socrates 0:b13a317308d1 16
Socrates 0:b13a317308d1 17 void setlooptimerflag(void)
Socrates 0:b13a317308d1 18 {
Socrates 0:b13a317308d1 19 looptimerflag = true;
Socrates 0:b13a317308d1 20 }
Socrates 0:b13a317308d1 21
Socrates 15:ab236d7c32d2 22 volatile bool dirflag=true;
Socrates 13:e0e9fda0e9a1 23 void tricheck(void)
Socrates 13:e0e9fda0e9a1 24 {
Socrates 15:ab236d7c32d2 25 dirflag=true;
Socrates 13:e0e9fda0e9a1 26 }
Socrates 13:e0e9fda0e9a1 27
Socrates 0:b13a317308d1 28 int main()
Socrates 0:b13a317308d1 29 {
Socrates 13:e0e9fda0e9a1 30 pwm_x.period(1.0/22000.0);
Socrates 13:e0e9fda0e9a1 31 pwm_y.period(1.0/22000.0);
Socrates 0:b13a317308d1 32 Ticker looptimer;
Socrates 15:ab236d7c32d2 33 Timeout dirtimeout;
Socrates 12:100cbba6cd96 34 const double ts=0.001;
Socrates 1:34202d107458 35 looptimer.attach(setlooptimerflag,ts);
Socrates 12:100cbba6cd96 36 double numh1,numh2,denh2,numl1,numl2,numl3,denl2,denl3;
Socrates 12:100cbba6cd96 37 double xtr,ytr,y1tr,x1tr,ztr,z1tr,z2tr,yabstr,yabs1tr,yabs2tr,ktr;
Socrates 12:100cbba6cd96 38 double xbr,ybr,y1br,x1br,zbr,z1br,z2br,yabsbr,yabs1br,yabs2br,kbr;
Socrates 12:100cbba6cd96 39 double xtl,ytl,y1tl,x1tl,ztl,z1tl,z2tl,yabstl,yabs1tl,yabs2tl,ktl;
Socrates 12:100cbba6cd96 40 double xbl,ybl,y1bl,x1bl,zbl,z1bl,z2bl,yabsbl,yabs1bl,yabs2bl,kbl;
Socrates 12:100cbba6cd96 41 double zx,zy;
Socrates 15:ab236d7c32d2 42 int xdir;
Socrates 15:ab236d7c32d2 43 int ydir;
Socrates 8:43cce9f7a006 44
Socrates 8:43cce9f7a006 45 x1tr=0; y1tr=0; z1tr=0; z2tr=0; yabs1tr=0; yabs2tr=0;
Socrates 8:43cce9f7a006 46 x1br=0; y1br=0; z1br=0; z2br=0; yabs1br=0; yabs2br=0;
Socrates 8:43cce9f7a006 47 x1tl=0; y1tl=0; z1tl=0; z2tl=0; yabs1tl=0; yabs2tl=0;
Socrates 8:43cce9f7a006 48 x1bl=0; y1bl=0; z1bl=0; z2bl=0; yabs1bl=0; yabs2bl=0;
Socrates 9:006f34c9f750 49 zx=0; zy=0;
Socrates 15:ab236d7c32d2 50 xdir=0; ydir=0;
Socrates 2:3dab90d3aac2 51
Socrates 6:27a4e8d9ddac 52 //High pass, 35Hz, 1e
Socrates 6:27a4e8d9ddac 53 numh1=0.900575535279376;
Socrates 6:27a4e8d9ddac 54 numh2=-0.900575535279376;
Socrates 2:3dab90d3aac2 55 //denh1=1;
Socrates 6:27a4e8d9ddac 56 denh2=-0.801151070558751;
Socrates 6:27a4e8d9ddac 57
Socrates 6:27a4e8d9ddac 58 //Low pass, 2 Hz, 2e orde
Socrates 4:513c33bba011 59 numl1=0.391302053991682*pow(10.0,-4.0);
Socrates 4:513c33bba011 60 numl2=0.782604107983365*pow(10.0,-4.0);
Socrates 4:513c33bba011 61 numl3=0.391302053991682*pow(10.0,-4.0);
Socrates 2:3dab90d3aac2 62 //denl1=1;
Socrates 4:513c33bba011 63 denl2=-1.982228929792529;
Socrates 4:513c33bba011 64 denl3=0.982385450614126;
Socrates 0:b13a317308d1 65 pc.baud(115200);
Socrates 0:b13a317308d1 66
Socrates 0:b13a317308d1 67 while(1) {
Socrates 13:e0e9fda0e9a1 68 while(looptimerflag != true);{
Socrates 13:e0e9fda0e9a1 69 }
Socrates 0:b13a317308d1 70 looptimerflag = false;
Socrates 6:27a4e8d9ddac 71
Socrates 8:43cce9f7a006 72 ktr=emgtr.read();
Socrates 8:43cce9f7a006 73 xtr=(ktr-0.5)*2.0;
Socrates 8:43cce9f7a006 74 ytr=xtr*numh1+x1tr*numh2-y1tr*denh2;
Socrates 8:43cce9f7a006 75 yabstr=abs(ytr);
Socrates 8:43cce9f7a006 76 ztr=yabstr*numl1+yabs1tr*numl2+yabs2tr*numl3-z1tr*denl2-z2tr*denl3;
Socrates 8:43cce9f7a006 77 x1tr=xtr; y1tr=ytr; z2tr=z1tr; z1tr=ztr; yabs2tr=yabs1tr; yabs1tr=yabstr;
Socrates 1:34202d107458 78
Socrates 8:43cce9f7a006 79 kbr=emgbr.read();
Socrates 8:43cce9f7a006 80 xbr=(kbr-0.5)*2.0;
Socrates 8:43cce9f7a006 81 ybr=xbr*numh1+x1br*numh2-y1br*denh2;
Socrates 8:43cce9f7a006 82 yabsbr=abs(ybr);
Socrates 8:43cce9f7a006 83 zbr=yabsbr*numl1+yabs1br*numl2+yabs2br*numl3-z1br*denl2-z2br*denl3;
Socrates 8:43cce9f7a006 84 x1br=xbr; y1br=ybr; z2br=z1br; z1br=zbr; yabs2br=yabs1br; yabs1br=yabsbr;
Socrates 6:27a4e8d9ddac 85
Socrates 8:43cce9f7a006 86 ktl=emgtl.read();
Socrates 8:43cce9f7a006 87 xtl=(ktl-0.5)*2.0;
Socrates 8:43cce9f7a006 88 ytl=xtl*numh1+x1tl*numh2-y1tl*denh2;
Socrates 8:43cce9f7a006 89 yabstl=abs(ytl);
Socrates 8:43cce9f7a006 90 ztl=yabstl*numl1+yabs1tl*numl2+yabs2tl*numl3-z1tl*denl2-z2tl*denl3;
Socrates 8:43cce9f7a006 91 x1tl=xtl; y1tl=ytl; z2tl=z1tl; z1tl=ztl; yabs2tl=yabs1tl; yabs1tl=yabstl;
Socrates 5:1a1ce2f5cb66 92
Socrates 8:43cce9f7a006 93 kbl=emgbl.read();
Socrates 8:43cce9f7a006 94 xbl=(kbl-0.5)*2.0;
Socrates 8:43cce9f7a006 95 ybl=xbl*numh1+x1bl*numh2-y1bl*denh2;
Socrates 8:43cce9f7a006 96 yabsbl=abs(ybl);
Socrates 8:43cce9f7a006 97 zbl=yabsbl*numl1+yabs1bl*numl2+yabs2bl*numl3-z1bl*denl2-z2bl*denl3;
Socrates 8:43cce9f7a006 98 x1bl=xbl; y1bl=ybl; z2bl=z1bl; z1bl=zbl; yabs2bl=yabs1bl; yabs1bl=yabsbl;
Socrates 8:43cce9f7a006 99
Socrates 15:ab236d7c32d2 100 zx=(zbr*3.5);
Socrates 6:27a4e8d9ddac 101
Socrates 13:e0e9fda0e9a1 102 //if (ztl>zbl) {
Socrates 13:e0e9fda0e9a1 103 // zy=ztl*5.0;
Socrates 13:e0e9fda0e9a1 104 // ydir=0;
Socrates 13:e0e9fda0e9a1 105 //} else {
Socrates 13:e0e9fda0e9a1 106 // zy=zbl*5.0;
Socrates 13:e0e9fda0e9a1 107 // ydir=1;
Socrates 13:e0e9fda0e9a1 108 //}
Socrates 8:43cce9f7a006 109
Socrates 8:43cce9f7a006 110 if (zx>1.0) {
Socrates 9:006f34c9f750 111 zx=0.99999;
Socrates 8:43cce9f7a006 112 }
Socrates 8:43cce9f7a006 113 if (zy>1.0) {
Socrates 9:006f34c9f750 114 zy=0.99999;
Socrates 8:43cce9f7a006 115 }
Socrates 8:43cce9f7a006 116
Socrates 15:ab236d7c32d2 117 //if (zx<0.20){
Socrates 15:ab236d7c32d2 118 //zx=0;
Socrates 15:ab236d7c32d2 119 //}
Socrates 11:28a196a3d898 120 if (zy<0.20){
Socrates 9:006f34c9f750 121 zy=0;
Socrates 9:006f34c9f750 122 }
Socrates 9:006f34c9f750 123
Socrates 15:ab236d7c32d2 124 if ((ztr>(zbr+0.1)) && dirflag == true) {
Socrates 15:ab236d7c32d2 125 dirflag = false;
Socrates 15:ab236d7c32d2 126 xdir ^= 1;
Socrates 15:ab236d7c32d2 127 dirtimeout.attach(tricheck,1.5);
Socrates 15:ab236d7c32d2 128 }
Socrates 15:ab236d7c32d2 129 //motordir1.write(xdir);
Socrates 15:ab236d7c32d2 130 //motordir2.write(ydir);
Socrates 15:ab236d7c32d2 131
Socrates 11:28a196a3d898 132 pwm_x.write(abs(zx));
Socrates 11:28a196a3d898 133 pwm_y.write(abs(zy));
Socrates 15:ab236d7c32d2 134 pc.printf("EMGRechts: %f, EMGLinks: %f, Richting: %d \n\r",zbr*3.0,ztr*3.0,xdir);
Socrates 0:b13a317308d1 135 }
Socrates 0:b13a317308d1 136 }