Diff: main.cpp

Revision:

24:fc01d9125d3b

Parent:

23:aae5cbe3b924

Child:

25:73c2eedb3b91

--- a/main.cpp	Fri Feb 28 00:23:00 2014 +0000
+++ b/main.cpp	Fri Feb 28 01:07:14 2014 +0000
@@ -145,11 +145,68 @@
     reckoner.reset();
 }
 
+// Keep the robot pointing the in the right direction (1, 0).
+// This should basically keep it going straight.
+void updateMotorsToDriveStraight()
+{
+    const int32_t straightDriveStrength = 1000;
+    int16_t speedLeft = drivingSpeed;
+    int16_t speedRight = drivingSpeed;
+    int32_t reduction = reckoner.sin / (1<<15) * straightDriveStrength / (1 << 15);
+    if (reduction > 0)
+    {
+        speedRight = reduceSpeed(speedRight, reduction);
+    }
+    else
+    {
+        speedLeft = reduceSpeed(speedLeft, -reduction);
+    }
+    motorsSpeedSet(speedLeft, speedRight);   
+}
+
+void loadCalibrationAndFindLine()
+{
+    lineTracker.calibratedMinimum[0] = 34872;
+    lineTracker.calibratedMinimum[1] = 29335;
+    lineTracker.calibratedMinimum[2] = 23845;
+    lineTracker.calibratedMaximum[0] = 59726;
+    lineTracker.calibratedMaximum[1] = 60110;
+    lineTracker.calibratedMaximum[2] = 58446;
+    
+    Timer foundLineTimer;
+    foundLineTimer.start();
+    bool foundLine = false;
+    
+    GeneralDebouncer lineStatus(10000);
+    while(1)
+    {
+        lineTracker.read();
+        lineTracker.updateCalibration();       
+        updateReckonerFromEncoders();
+        updateMotorsToDriveStraight();        
+        lineStatus.update(lineTracker.getLineVisible());       
+
+        if (foundLine)
+        {
+            if(foundLineTimer.read_ms() >= 500)
+            {
+                // We found the line and traveled for a bit more, so now we can be done.
+                break;
+            }
+        }
+        else
+        {
+            if(lineStatus.getState() == true && lineStatus.getTimeInCurrentStateMicroseconds() > 150000)
+            {
+                foundLine = true;
+                foundLineTimer.start();
+            }
+        }
+    }
+}
 
 void findLineAndCalibrate()
 {
-    const int32_t straightDriveStrength = 1000;
-    
     Timer timer;
     timer.start();
     
@@ -160,21 +217,8 @@
     {
         lineTracker.read();
         lineTracker.updateCalibration();       
-        updateReckonerFromEncoders();
-        
-        // Keep the robot pointing the in the right direction.  This should basically keep it going straight.
-        int16_t speedLeft = drivingSpeed;
-        int16_t speedRight = drivingSpeed;
-        int32_t reduction = reckoner.sin / (1<<15) * straightDriveStrength / (1 << 15);
-        if (reduction > 0)
-        {
-            speedRight = reduceSpeed(speedRight, reduction);
-        }
-        else
-        {
-            speedLeft = reduceSpeed(speedLeft, -reduction);
-        }
-        motorsSpeedSet(speedLeft, speedRight);
+        updateReckonerFromEncoders();        
+        updateMotorsToDriveStraight();
         
         if (goodCalibration)
         {