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@10:2e66843bb6d7, 2013-10-21 (annotated)

Committer:
Socrates
Date:
Mon Oct 21 13:52:39 2013 +0000
Revision:
10:2e66843bb6d7
Parent:
9:006f34c9f750
Child:
11:28a196a3d898
4Filters, ietwat opgeschoond;

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