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@11:28a196a3d898, 2013-10-23 (annotated)

Committer:
Socrates
Date:
Wed Oct 23 13:32:39 2013 +0000
Revision:
11:28a196a3d898
Parent:
10:2e66843bb6d7
Child:
12:100cbba6cd96
Werkt op x, met duty cycle

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