Diff: main.cpp

Revision:

4:14b00a82d206

Parent:

3:6f773c2ba3a0

Child:

5:f3805a1f9047

Child:

7:d0c19729e7fd

--- a/main.cpp	Wed May 21 13:54:40 2014 +0000
+++ b/main.cpp	Wed May 21 15:09:12 2014 +0000
@@ -16,9 +16,20 @@
     huey.printf("%f",bat); 
 }
 
+float myLinePos(void)
+{
+    int sensors[5];
+    huey.calibrated_sensor(sensors);
+    float leftSen, rightSen, midSen;
+    leftSen = sensors[1] / 1000;
+    rightSen = sensors[3] / 1000;
+    midSen = sensors[2] / 1000;
+    return rightSen - leftSen;
+}
+
 void smoothFollow(float position, float speed)
 {
-    float u = speed/1.;
+    float u = speed;
     u = u * position;
     if(speed+u > 1)
     {
@@ -49,7 +60,6 @@
 {
     DigitalIn button(p21);
     int value = button.read();
-    huey.printf("%d", value);
     return value;
 }
 
@@ -79,7 +89,7 @@
     while(z==1)
     {
         //huey.right_motor(speed);
-        pos = huey.line_position();
+        pos = myLinePos();//huey.line_position();
         smoothFollow(pos, speed);
         if(m3pi_IN[0]==0)
         {