Diff: main.cpp

Revision:

50:517c0f0e621f

Parent:

49:eaa6fd514f4f

Child:

51:b9f7243609d4

--- a/main.cpp	Wed Apr 15 21:58:21 2015 +0000
+++ b/main.cpp	Wed Apr 15 22:53:33 2015 +0000
@@ -74,8 +74,9 @@
     setLeds(1, 0, 0, 0);
     waitForSignalToStart();
         
-    setLeds(0, 1, 0, 0);   
-    followLineFast();
+    setLeds(0, 1, 0, 0);    // led4 gets set when it detects the start
+    //followLineFast();
+    followLineSmart();
     
     setLeds(1, 1, 1, 1);
     loggerReportLoop();
@@ -284,22 +285,15 @@
     Pacer reportPacer(200000);
     
     loadCalibration();
-    uint32_t loopCount = 0;
     Timer timer;
     timer.start();
     turnSensor.start();
     while(1)
     {
-        loopCount += 1;
         turnSensor.update();
         updateReckoner(turnSensor);
         loggerService();
         
-        if ((loopCount % 256) == 0)
-        {
-            pc.printf("%d\r\n", lineTracker.getLinePosition());
-        }
-        
         lineTracker.read();
         updateMotorsToFollowLineFast();
         
@@ -311,3 +305,44 @@
     motorsSpeedSet(0, 0);
 }
 
+bool foundStart()
+{
+    static int16_t lastX = 0;
+    return lastX < 0 && reckoner.x >= 0 && abs(reckoner.y) < (85 << 16) &&
+      totalEncoderCounts > 10000 && abs(turnSensor.getAngle()) < turnAngle1 * 30;
+    lastX = reckoner.x;
+}
+
+void followLineSmart()
+{
+    totalEncoderCounts = 0;
+    Pacer reportPacer(200000);
+    
+    loadCalibration();
+    turnSensor.start();
+    while(1)
+    {
+        turnSensor.update();
+        updateReckoner(turnSensor);
+        loggerService();
+        
+        lineTracker.read();
+        updateMotorsToFollowLineFast();
+
+        if (foundStart())
+        {
+            reckoner.reset();
+            turnSensor.reset();
+            totalEncoderCounts = 0;
+            nextLogEncoderCount = 0;
+            led4 = 1;
+        }        
+        
+        if (button1DefinitelyPressed())
+        {
+            break;
+        }
+    }
+    motorsSpeedSet(0, 0);
+}
+