Diff: main.cpp

Revision:

28:4374035df5e0

Parent:

27:2456f68be679

Child:

29:cfcf08d8ac79

--- a/main.cpp	Sat Mar 01 01:46:35 2014 +0000
+++ b/main.cpp	Sat Mar 01 03:13:57 2014 +0000
@@ -44,13 +44,17 @@
     //testDriveHome();
     //testFinalSettleIn();
     //testCalibrate();
+    //testLineFollowing();
+    testAnalog();
 
     // Real routines for the contest.
+    loadCalibration();
+    
     setLeds(1, 0, 0, 0);
     waitForSignalToStart();
     setLeds(0, 1, 0, 0);
     
-    loadCalibrationAndFindLine();
+    findLine();
     //findLineAndCalibrate();
     
     //setLeds(1, 1, 0, 0);
@@ -65,6 +69,16 @@
     while(1){}
 }
 
+void loadCalibration()
+{
+    lineTracker.calibratedMinimum[0] = 34872;
+    lineTracker.calibratedMinimum[1] = 29335;
+    lineTracker.calibratedMinimum[2] = 23845;
+    lineTracker.calibratedMaximum[0] = 59726;
+    lineTracker.calibratedMaximum[1] = 60110;
+    lineTracker.calibratedMaximum[2] = 58446;   
+}
+
 void updateReckonerFromEncoders()
 {
     while(encoderBuffer.hasEvents())
@@ -167,15 +181,27 @@
     motorsSpeedSet(speedLeft, speedRight);   
 }
 
-void loadCalibrationAndFindLine()
+void updateMotorsToFollowLine()
 {
-    lineTracker.calibratedMinimum[0] = 34872;
-    lineTracker.calibratedMinimum[1] = 29335;
-    lineTracker.calibratedMinimum[2] = 23845;
-    lineTracker.calibratedMaximum[0] = 59726;
-    lineTracker.calibratedMaximum[1] = 60110;
-    lineTracker.calibratedMaximum[2] = 58446;
+    const int followLineStrength = 300;
+
+    int16_t speedLeft = drivingSpeed;
+    int16_t speedRight = drivingSpeed;
+    int16_t reduction = (lineTracker.getLinePosition() - 1000) * followLineStrength / 1000;
+    if(reduction < 0)
+    {
+        speedLeft = reduceSpeed(speedLeft, -reduction);   
+    }
+    else
+    {
+        speedRight = reduceSpeed(speedRight, reduction);
+    }
     
+    motorsSpeedSet(speedLeft, speedRight);      
+}
+
+void findLine()
+{   
     GeneralDebouncer lineStatus(10000);
     while(1)
     {
@@ -252,7 +278,6 @@
     
     GeneralDebouncer lineStatus(10000);
     const uint32_t lineDebounceTime = 100000;
-    const int followLineStrength = 300;
     
     while(1)
     {
@@ -263,25 +288,12 @@
         
         bool lostLine = lineStatus.getState() == false &&
           lineStatus.getTimeInCurrentStateMicroseconds() > lineDebounceTime;        
-        if(lostLine && timer.read_us() >= 300000)
+        if(lostLine && timer.read_us() >= 2000000)
         {
             break;   
         }
         
-        int16_t speedLeft = drivingSpeed;
-        int16_t speedRight = drivingSpeed;
-        int16_t reduction = (lineTracker.getLinePosition() - 1500) * followLineStrength / 1500;
-        if(reduction < 0)
-        {
-            speedLeft = reduceSpeed(speedLeft, -reduction);   
-        }
-        else
-        {
-            speedRight = reduceSpeed(speedRight, reduction);
-        }
-        
-        
-        motorsSpeedSet(speedLeft, speedRight);        
+        updateMotorsToFollowLine();     
     }
 }