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

--- 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