Revision 8:43cce9f7a006, committed 2013-10-21

Comitter:

Socrates

Date:

Mon Oct 21 09:02:46 2013 +0000

Parent:

7:4d2edb5b0164

Child:

9:006f34c9f750

Commit message:

Filters voor 4.

Changed in this revision

--- a/main.cpp	Fri Oct 18 10:05:56 2013 +0000
+++ b/main.cpp	Mon Oct 21 09:02:46 2013 +0000
@@ -1,10 +1,11 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
-AnalogIn emgt(PTB1);
-AnalogIn emgb(PTB0);
+AnalogIn emgtr(PTB3);
+AnalogIn emgbr(PTB2);
+AnalogIn emgtl(PTB1);
+AnalogIn emgbl(PTB0);
 MODSERIAL pc(USBTX,USBRX);
-PwmOut pwm_motor1(PTA12);
-PwmOut pwm_motor2(PTA5);
+
 volatile bool looptimerflag;
 
 void setlooptimerflag(void)
@@ -12,29 +13,22 @@
     looptimerflag = true;
 }
 
-
 int main()
 {
     Ticker looptimer;
     const float ts=0.001;
     looptimer.attach(setlooptimerflag,ts);
-    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;
+    float numh1,numh2,denh2,numl1,numl2,numl3,denl2,denl3;
+    float xtr,ytr,y1tr,x1tr,ztr,z1tr,z2tr,yabstr,yabs1tr,yabs2tr,ktr;
+    float xbr,ybr,y1br,x1br,zbr,z1br,z2br,yabsbr,yabs1br,yabs2br,kbr;
+    float xtl,ytl,y1tl,x1tl,ztl,z1tl,z2tl,yabstl,yabs1tl,yabs2tl,ktl;
+    float xbl,ybl,y1bl,x1bl,zbl,z1bl,z2bl,yabsbl,yabs1bl,yabs2bl,kbl;
+    float zx,zy,xdir,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;
 
     //High pass, 35Hz, 1e
     numh1=0.900575535279376;
@@ -55,63 +49,59 @@
         while(looptimerflag != true);
         looptimerflag = false;
 
-        //Triceps
-        kt=emgt.read();
-        xt=(kt-0.5)*2.0;
-
-        //High pass
-        //1e orde
-        yt=xt*numh1+x1t*numh2-y1t*denh2;
-
-        //Rectify
-        yabst=abs(yt);
+ktr=emgtr.read();
+        xtr=(ktr-0.5)*2.0;
+        ytr=xtr*numh1+x1tr*numh2-y1tr*denh2;
+        yabstr=abs(ytr);
+        ztr=yabstr*numl1+yabs1tr*numl2+yabs2tr*numl3-z1tr*denl2-z2tr*denl3;
+        x1tr=xtr;        y1tr=ytr;        z2tr=z1tr;        z1tr=ztr;        yabs2tr=yabs1tr;        yabs1tr=yabstr;
 
-        //Low pass
-        //2e orde
-        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;
+kbr=emgbr.read();
+        xbr=(kbr-0.5)*2.0;
+        ybr=xbr*numh1+x1br*numh2-y1br*denh2;
+        yabsbr=abs(ybr);
+        zbr=yabsbr*numl1+yabs1br*numl2+yabs2br*numl3-z1br*denl2-z2br*denl3;
+        x1br=xbr;        y1br=ybr;        z2br=z1br;        z1br=zbr;        yabs2br=yabs1br;        yabs1br=yabsbr;
 
-        //Biceps
-        kb=emgb.read();
-        xb=(kb-0.5)*2.0;
+ktl=emgtl.read();
+        xtl=(ktl-0.5)*2.0;
+        ytl=xtl*numh1+x1tl*numh2-y1tl*denh2;
+        yabstl=abs(ytl);
+        ztl=yabstl*numl1+yabs1tl*numl2+yabs2tl*numl3-z1tl*denl2-z2tl*denl3;
+        x1tl=xtl;        y1tl=ytl;        z2tl=z1tl;        z1tl=ztl;        yabs2tl=yabs1tl;        yabs1tl=yabstl;
 
-        //High pass
-        //1e orde
-        yb=xb*numh1+x1b*numh2-y1b*denh2;
-
-        //Rectify
-        yabsb=abs(yb);
 
-        //Low pass
-        //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;
+kbl=emgbl.read();
+        xbl=(kbl-0.5)*2.0;
+        ybl=xbl*numh1+x1bl*numh2-y1bl*denh2;
+        yabsbl=abs(ybl);
+        zbl=yabsbl*numl1+yabs1bl*numl2+yabs2bl*numl3-z1bl*denl2-z2bl*denl3;
+        x1bl=xbl;        y1bl=ybl;        z2bl=z1bl;        z1bl=zbl;        yabs2bl=yabs1bl;        yabs1bl=yabsbl;
+        
+        if (ztr>zbr) {
+            zx=ztr;
+            xdir=0;
         } else {
-            z=zt;
+            zx=zbr;
+            xdir=1;
         }
 
-        if (z>1.0) {
-            z=1.0;
+ if (ztl>zbl) {
+            zy=ztl;
+            ydir=0;
+        } else {
+            zy=zbl;
+            ydir=1;
         }
-        pwm_motor1=z;
-        pwm_motor2=z;
-        pc.printf("%f\n\r",z);
+        
+        if (zx>1.0) {
+            zx=1.0;
+        }
+        if (zy>1.0) {
+            zy=1.0;
+        }
+        
+        pc.printf("X: %f Y: %f\n\r",zx*5.0,zy*5.0);
     }
 }
\ No newline at end of file