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@14:5746b1697ff4, 2013-10-24 (annotated)

Committer:
gerard1993
Date:
Thu Oct 24 13:14:44 2013 +0000
Revision:
14:5746b1697ff4
Parent:
13:e0e9fda0e9a1
Child:
16:414a2397c493
hoi

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