Diff: main.cpp

Revision:

12:100cbba6cd96

Parent:

11:28a196a3d898

Child:

13:e0e9fda0e9a1

diff -r 28a196a3d898 -r 100cbba6cd96 main.cpp
--- a/main.cpp	Wed Oct 23 13:32:39 2013 +0000
+++ b/main.cpp	Wed Oct 23 15:04:34 2013 +0000
@@ -9,7 +9,8 @@
 PwmOut pwm_y(PTA12);
 //AnalogOut pwm_x(PTE30);
 MODSERIAL pc(USBTX,USBRX);
-
+DigitalOut motordir1(PTD3);
+DigitalOut motordir2(PTD1);
 
 volatile bool looptimerflag;
 
@@ -23,15 +24,15 @@
 pwm_x.period(1.0/1000.0);
 pwm_y.period(1.0/1000.0);
     Ticker looptimer;
-    const float ts=0.001;
+    const double ts=0.001;
     looptimer.attach(setlooptimerflag,ts);
-    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;
-    float xdir,ydir;
+    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;
 
     x1tr=0;    y1tr=0;    z1tr=0;    z2tr=0;    yabs1tr=0;    yabs2tr=0;
     x1br=0;    y1br=0;    z1br=0;    z2br=0;    yabs1br=0;    yabs2br=0;
@@ -87,7 +88,7 @@
         x1bl=xbl;        y1bl=ybl;        z2bl=z1bl;        z1bl=zbl;        yabs2bl=yabs1bl;        yabs1bl=yabsbl;
         
         if (ztr>zbr) {
-            zx=ztr*2.5;
+            zx=ztr*3.5;
             xdir=0;
         } else {
             zx=zbr*2.5;
@@ -116,6 +117,8 @@
         zy=0;
         }
         
+        motordir1.write(xdir);
+        motordir2.write(ydir);
         pwm_x.write(abs(zx));
         pwm_y.write(abs(zy));
         pc.printf("%f\n\r",zx);