Eindversie. LU: 07-11-13.
Dependencies: MODSERIAL mbed Encoder
Revision 15:ab236d7c32d2, committed 2013-10-25
- Comitter:
- Socrates
- Date:
- Fri Oct 25 09:41:14 2013 +0000
- Parent:
- 13:e0e9fda0e9a1
- Child:
- 16:414a2397c493
- Commit message:
- Nu met timeout
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Oct 24 12:48:06 2013 +0000 +++ b/main.cpp Fri Oct 25 09:41:14 2013 +0000 @@ -18,13 +18,10 @@ looptimerflag = true; } -double ztr,zbr,xdir; - +volatile bool dirflag=true; void tricheck(void) { -if (ztr>zbr) { - xdir = !xdir; - } +dirflag=true; } int main() @@ -32,23 +29,24 @@ pwm_x.period(1.0/22000.0); pwm_y.period(1.0/22000.0); Ticker looptimer; - Ticker looptimer2; + Timeout dirtimeout; const double ts=0.001; looptimer.attach(setlooptimerflag,ts); - looptimer2.attach(tricheck,1); double numh1,numh2,denh2,numl1,numl2,numl3,denl2,denl3; double xtr,ytr,y1tr,x1tr,ztr,z1tr,z2tr,yabstr,yabs1tr,yabs2tr,ktr; double xbr,ybr,y1br,x1br,zbr,z1br,z2br,yabsbr,yabs1br,yabs2br,kbr; double xtl,ytl,y1tl,x1tl,ztl,z1tl,z2tl,yabstl,yabs1tl,yabs2tl,ktl; double xbl,ybl,y1bl,x1bl,zbl,z1bl,z2bl,yabsbl,yabs1bl,yabs2bl,kbl; double zx,zy; - double xdir,ydir; + int xdir; + int ydir; x1tr=0; y1tr=0; z1tr=0; z2tr=0; yabs1tr=0; yabs2tr=0; x1br=0; y1br=0; z1br=0; z2br=0; yabs1br=0; yabs2br=0; x1tl=0; y1tl=0; z1tl=0; z2tl=0; yabs1tl=0; yabs2tl=0; x1bl=0; y1bl=0; z1bl=0; z2bl=0; yabs1bl=0; yabs2bl=0; zx=0; zy=0; + xdir=0; ydir=0; //High pass, 35Hz, 1e numh1=0.900575535279376; @@ -98,10 +96,7 @@ zbl=yabsbl*numl1+yabs1bl*numl2+yabs2bl*numl3-z1bl*denl2-z2bl*denl3; x1bl=xbl; y1bl=ybl; z2bl=z1bl; z1bl=zbl; yabs2bl=yabs1bl; yabs1bl=yabsbl; - zx=zbr*3.5; - if (ztr>zbr) { - xdir = !xdir; - } + zx=(zbr*3.5); //if (ztl>zbl) { // zy=ztl*5.0; @@ -118,17 +113,23 @@ zy=0.99999; } - if (zx<0.20){ - zx=0; - } + //if (zx<0.20){ + //zx=0; + //} if (zy<0.20){ zy=0; } - motordir1.write(xdir); - motordir2.write(ydir); + if ((ztr>(zbr+0.1)) && dirflag == true) { + dirflag = false; + xdir ^= 1; + dirtimeout.attach(tricheck,1.5); + } + //motordir1.write(xdir); + //motordir2.write(ydir); + pwm_x.write(abs(zx)); pwm_y.write(abs(zy)); - pc.printf("EMG: %f, Richting: %d \n\r",zx, xdir); + pc.printf("EMGRechts: %f, EMGLinks: %f, Richting: %d \n\r",zbr*3.0,ztr*3.0,xdir); } } \ No newline at end of file