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

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