Diff: main.cpp

Revision:

52:05a8e919ddb0

Parent:

51:b9f7243609d4

Child:

53:73565a3ef5c5

--- a/main.cpp	Wed Apr 15 23:01:39 2015 +0000
+++ b/main.cpp	Wed Apr 15 23:56:52 2015 +0000
@@ -21,6 +21,7 @@
 Logger logger;
 Pacer loggerPacer(50000);
 
+uint8_t lapsCompleted = 0;
 uint32_t totalEncoderCounts = 0;
 uint32_t nextLogEncoderCount = 0;
 const uint32_t logSpacing = 100;
@@ -73,8 +74,8 @@
     
     setLeds(1, 0, 0, 0);
     waitForSignalToStart();
-        
-    setLeds(0, 1, 0, 0);    // led4 gets set when it detects the start
+  
+    setLeds(0, 1, 0, 0);    // led3 and led4 get set by followLineSmart for debugging
     //followLineFast();
     followLineSmart();
     
@@ -234,7 +235,7 @@
     wait(0.2);
 }
 
-void updateMotorsToFollowLine()
+void updateMotorsToFollowLineSlow()
 {
     const int16_t drivingSpeed = 400;
     const int32_t followLineStrength = drivingSpeed * 5 / 4;
@@ -254,9 +255,8 @@
     motorsSpeedSet(speedLeft, speedRight);
 }
 
-void updateMotorsToFollowLineFast()
+void updateMotorsToFollowLineFast(int16_t drivingSpeed)
 {
-    const int16_t drivingSpeed = 1000;
     const int32_t followLineStrength = drivingSpeed * 5 / 4;
     static int16_t lastPosition = 1000;
 
@@ -295,7 +295,7 @@
         loggerService();
         
         lineTracker.read();
-        updateMotorsToFollowLineFast();
+        updateMotorsToFollowLineFast(1000);
         
         if (button1DefinitelyPressed())
         {
@@ -314,9 +314,59 @@
     return result;
 }
 
+bool onLongStraightPart()
+{
+    if (lapsCompleted == 0) { return false; }
+    
+    // Figure out what part of the log corresponds to our current situation.
+    uint32_t logIndex = totalEncoderCounts / logSpacing;
+    
+    if (logIndex >= logger.getSize())
+    {
+        // Should not happen.
+        return false;
+    }
+    
+    // To improve this, we could check that turnSensor.getAngle() matches what is in the log.
+
+    uint32_t angle1 = turnSensor.getAngleUnsigned();
+
+    // 1000 encoder ticks
+    const uint32_t lookAheadAmount = 2000 / logSpacing;
+    
+    // Figure out how far away the next turn is.
+    uint32_t i = logIndex;
+    while(1)
+    {
+        i++;
+        
+        if (i >= logger.getSize())
+        {
+            // reached the end the log
+            return false;
+        }
+
+        if (i > logIndex + lookAheadAmount)
+        {
+            // looked far enough ahead that we don't think there is a turn coming up soon
+            return true;
+        }
+        
+        
+        uint32_t angle2 = (uint16_t)logger.entries[i].turnAngle << 16;
+        if (abs((int32_t)(angle2 - angle1)) > turnAngle45)
+        {
+            // detected a turn
+            return false;
+        }
+    }
+}
+
 void followLineSmart()
 {
+    lapsCompleted = 0;   
     totalEncoderCounts = 0;
+
     Pacer reportPacer(200000);
     
     loadCalibration();
@@ -328,16 +378,42 @@
         loggerService();
         
         lineTracker.read();
-        updateMotorsToFollowLineFast();
+        
+        // By default, choose a cautious speed of 1000 (out of 1200).
+        int16_t speed = 1000;
+        
+        // Go fast if we know we are on a long straight part.
+        if (onLongStraightPart())
+        {
+            speed = 1200;
+            led3 = 1;
+        }
+        else
+        {
+            led3 = 0;
+        }
+        
+        
+        updateMotorsToFollowLineFast(speed);
 
         if (foundStart())
         {
+            // Another lap has been completed!
+            lapsCompleted = 1;
+            led4 = lapsCompleted & 1;
+
             reckoner.reset();
             turnSensor.reset();
             totalEncoderCounts = 0;
             nextLogEncoderCount = 0;
-            led4 = 1;
-        }        
+        }
+        
+        if (lapsCompleted == 3 && totalEncoderCounts > 2000)
+        {
+            // Classy move: know when you are done with the competition and stop automatically.
+            // (Of course, there is a risk that this will backfire!)
+            break;
+        }
         
         if (button1DefinitelyPressed())
         {