Eindversie. LU: 07-11-13.
Dependencies: MODSERIAL mbed Encoder
Revision 5:1a1ce2f5cb66, committed 2013-10-16
- Comitter:
- Socrates
- Date:
- Wed Oct 16 15:26:28 2013 +0000
- Parent:
- 4:513c33bba011
- Child:
- 6:27a4e8d9ddac
- Commit message:
- 2 EMGs, high 1e op 1, low 2e op 2 hz.
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Oct 16 15:00:40 2013 +0000 +++ b/main.cpp Wed Oct 16 15:26:28 2013 +0000 @@ -1,7 +1,7 @@ #include "mbed.h" #include "MODSERIAL.h" - -AnalogIn emg(PTB0); +AnalogIn emgt(PTB1); +AnalogIn emgb(PTB0); MODSERIAL pc(USBTX,USBRX); volatile bool looptimerflag; @@ -17,14 +17,23 @@ Ticker looptimer; const float ts=0.001; looptimer.attach(setlooptimerflag,ts); - float x,y,y1,y2,x1,z,z1,z2,yabs,yabs1,yabs2,k,numh1,numh2,denh1,denh2,numl1,numl2,numl3,denl1,denl2,denl3; - x1=0; - y1=0; - y2=0; - z1=0; - z2=0; - yabs1=0; - yabs2=0; + float xt,yt,y1t,y2t,x1t,zt,z1t,z2t,yabst,yabs1t,yabs2t,kt,numh1,numh2,denh1,denh2,numl1,numl2,numl3,denl1,denl2,denl3; + float xb,yb,y1b,y2b,x1b,zb,z1b,z2b,yabsb,yabs1b,yabs2b,kb; + float z; + x1t=0; + y1t=0; + y2t=0; + z1t=0; + z2t=0; + yabs1t=0; + yabs2t=0; + x1b=0; + y1b=0; + y2b=0; + z1b=0; + z2b=0; + yabs1b=0; + yabs2b=0; numh1=0.996868235770807; numh2=-0.996868235770807; @@ -41,27 +50,61 @@ while(1) { while(looptimerflag != true); looptimerflag = false; - k=emg.read(); - x=(k-0.5)*2.0; + //Triceps + kt=emgt.read(); + xt=(kt-0.5)*2.0; //High pass, 1 Hz //1e orde - y=x*numh1+x1*numh2-y1*denh2; + yt=xt*numh1+x1t*numh2-y1t*denh2; //Rectify - yabs=abs(y); + yabst=abs(yt); //Low pass, 2 Hz //2e orde - z=yabs*numl1+yabs1*numl2+yabs2*numl3-z1*denl2-z2*denl3; + zt=yabst*numl1+yabs1t*numl2+yabs2t*numl3-z1t*denl2-z2t*denl3; + + x1t=xt; + y2t=y1t; + y1t=yt; + z2t=z1t; + z1t=zt; + yabs2t=yabs1t; + yabs1t=yabst; + + //Biceps + kb=emgb.read(); + xb=(kb-0.5)*2.0; + + //High pass, 1 Hz + //1e orde + yb=xb*numh1+x1b*numh2-y1b*denh2; + + //Rectify + yabsb=abs(yb); + + //Low pass, 2 Hz + //2e orde + zb=yabsb*numl1+yabs1b*numl2+yabs2b*numl3-z1b*denl2-z2b*denl3; + + x1b=xb; + y2b=y1b; + y1b=yb; + z2b=z1b; + z1b=zb; + yabs2b=yabs1b; + yabs1b=yabsb; + + if (zb>zt) + { + z=zb; + } + else + { + z=zt; + } pc.printf("%f\n\r",z); - x1=x; - y2=y1; - y1=y; - z2=z1; - z1=z; - yabs2=yabs; - yabs1=yabs; } } \ No newline at end of file