Eindversie. LU: 07-11-13.
Dependencies: MODSERIAL mbed Encoder
Revision 9:006f34c9f750, committed 2013-10-21
- Comitter:
- Socrates
- Date:
- Mon Oct 21 11:48:58 2013 +0000
- Parent:
- 8:43cce9f7a006
- Child:
- 10:2e66843bb6d7
- Commit message:
- 4 EMG+filters, pwmout.
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Oct 21 09:02:46 2013 +0000 +++ b/main.cpp Mon Oct 21 11:48:58 2013 +0000 @@ -2,8 +2,11 @@ #include "MODSERIAL.h" AnalogIn emgtr(PTB3); AnalogIn emgbr(PTB2); +//Rechts is x, links is y AnalogIn emgtl(PTB1); AnalogIn emgbl(PTB0); +PwmOut pwm_x(PTA5); +PwmOut pwm_y(PTA12); MODSERIAL pc(USBTX,USBRX); volatile bool looptimerflag; @@ -29,6 +32,7 @@ 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; //High pass, 35Hz, 1e numh1=0.900575535279376; @@ -47,6 +51,10 @@ while(1) { while(looptimerflag != true); + { + pwm_x.write(abs(zx)); + //pwm_y.write(abs(zy)); + } looptimerflag = false; ktr=emgtr.read(); @@ -56,7 +64,6 @@ ztr=yabstr*numl1+yabs1tr*numl2+yabs2tr*numl3-z1tr*denl2-z2tr*denl3; x1tr=xtr; y1tr=ytr; z2tr=z1tr; z1tr=ztr; yabs2tr=yabs1tr; yabs1tr=yabstr; - kbr=emgbr.read(); xbr=(kbr-0.5)*2.0; ybr=xbr*numh1+x1br*numh2-y1br*denh2; @@ -71,7 +78,6 @@ ztl=yabstl*numl1+yabs1tl*numl2+yabs2tl*numl3-z1tl*denl2-z2tl*denl3; x1tl=xtl; y1tl=ytl; z2tl=z1tl; z1tl=ztl; yabs2tl=yabs1tl; yabs1tl=yabstl; - kbl=emgbl.read(); xbl=(kbl-0.5)*2.0; ybl=xbl*numh1+x1bl*numh2-y1bl*denh2; @@ -80,28 +86,37 @@ x1bl=xbl; y1bl=ybl; z2bl=z1bl; z1bl=zbl; yabs2bl=yabs1bl; yabs1bl=yabsbl; if (ztr>zbr) { - zx=ztr; + zx=ztr*5.0; xdir=0; } else { - zx=zbr; + zx=zbr*5.0; xdir=1; } - if (ztl>zbl) { - zy=ztl; + if (ztl>zbl) { + zy=ztl*5.0; ydir=0; } else { - zy=zbl; + zy=zbl*5.0; ydir=1; } if (zx>1.0) { - zx=1.0; + zx=0.99999; } if (zy>1.0) { - zy=1.0; + zy=0.99999; } - pc.printf("X: %f Y: %f\n\r",zx*5.0,zy*5.0); + if (zx<0.35){ + zx=0; + } + if (zy<0.35){ + zy=0; + } + + pwm_x.write(abs(zx)); + //pwm_y.write(abs(zy)); + pc.printf("%f\n\r",zx); } } \ No newline at end of file