Diff: Headers/Motors.h

Revision:

1:cfe6a6ad8dca

Parent:

0:c2ec30f28676

Child:

2:997f57aee3b7

--- a/Headers/Motors.h	Sat Mar 29 03:06:46 2014 +0000
+++ b/Headers/Motors.h	Sat Mar 29 13:25:23 2014 +0000
@@ -3,12 +3,15 @@
 #include "mbed.h"
 
 #define turn_time 157
-#define motor_wait 350
+#define motor_wait 700
 #define forward_time 190
 
+#include "PID.h"
+#include "Sensors.h"
+
+Serial Motor(p13,p14);
 
 
-Serial Motor(p13,p14);
 
 
 void setRightSpeed(int speed)
@@ -27,17 +30,13 @@
     else
         Motor.printf("1f%i\r", (-speed));
 }
-
-
-void forward(int f)
-{
-    setLeftSpeed(f);
-    wait_us(motor_wait);
-    setRightSpeed(f);
-}
 void stop()
 {
-    
+    setLeftSpeed(-5);
+    wait_us(motor_wait);
+    setRightSpeed(-5);
+    wait_us(motor_wait);
+    wait_us(20000);
     setLeftSpeed(0);
     wait_us(motor_wait);
     setRightSpeed(0);
@@ -45,6 +44,25 @@
   
 }
 
+void forward(int f)
+{
+    setLeftSpeed(f-1);
+    wait_us(motor_wait);
+    setRightSpeed(f);
+    wait_us(motor_wait);
+    setLeftSpeed(f);
+    wait_us(motor_wait);
+    
+    while(1)
+    {
+        WIRELESS.printf("L:%f  R:%f ::: PID: %f \n\r", valL, valR, PID());
+     
+    }
+    
+    stop();
+}
+
+
 
 
 void turn_right(int r)