Diff: main.cpp

Revision:

6:8f46c6ac55ca

Parent:

5:c705b7e7e1a8

Child:

7:218e861ea777

--- a/main.cpp	Wed Nov 10 09:05:50 2010 +0000
+++ b/main.cpp	Thu May 12 11:32:35 2011 +0000
@@ -1,8 +1,7 @@
 #include "mbed.h"
 #include "m3pi.h"
 
-BusOut leds(LED1,LED2,LED3,LED4);
-m3pi m3pi(p23,p9,p10);
+m3pi m3pi;
 
 int main() {
 
@@ -10,32 +9,33 @@
     m3pi.printf("Line Flw");
 
     wait(2.0);
-
-    float position_of_line = 0.0;
+    
     m3pi.sensor_auto_calibrate();
-    float speed = 0.4;
-
+    
+    float speed = 0.2;
+    float correction = 0.1;   
+    float threshold = 0.5;
+    
     while (1) {
 
-        // -1.0 is far left, 1.0 is far right
-        position_of_line = m3pi.line_position();
+        // -1.0 is far left, 1.0 is far right, 0.0 in the middle
+        float position_of_line = m3pi.line_position();
 
-        // Line is more than 25% to the left
-        if (position_of_line < -0.50) {
+        // Line is more than the threshold to the right, slow the left motor
+        if (position_of_line > threshold) {
+            m3pi.right_motor(speed);
+            m3pi.left_motor(speed-correction);
+        }
+
+        // Line is more than 50% to the left, slow the right motor
+        else if (position_of_line < -threshold) {
             m3pi.left_motor(speed);
-            m3pi.right_motor(speed - 0.3);
-            leds = 0x4;
+            m3pi.right_motor(speed-correction);
         }
-        // Line is more than 75% to the right
-        else if (position_of_line > 0.50) {
-            m3pi.right_motor(speed);
-            m3pi.left_motor(speed - 0.3);
-            leds = 0x2;
-        }
+
         // Line is in the middle
         else {
             m3pi.forward(speed);
-            leds = 0x0;
         }
     }
 }
\ No newline at end of file