Diff: main.cpp

Revision:

40:e79cefc241f8

Parent:

39:a5e25fd52ff8

Child:

41:4a4be80ff38c

--- a/main.cpp	Tue Apr 14 01:06:41 2015 +0000
+++ b/main.cpp	Tue Apr 14 01:45:01 2015 +0000
@@ -58,7 +58,7 @@
     setLeds(1, 0, 0, 0);
     waitForSignalToStart();
         
-    setLeds(0, 1, 0, 0);    
+    setLeds(0, 1, 0, 0);   
     followLineFast();
     
     setLeds(1, 1, 1, 1);
@@ -181,7 +181,7 @@
 void updateMotorsToFollowLine()
 {
     const int16_t drivingSpeed = 400;
-    const int followLineStrength = drivingSpeed * 5 / 4;
+    const int32_t followLineStrength = drivingSpeed * 5 / 4;
 
     int16_t speedLeft = drivingSpeed;
     int16_t speedRight = drivingSpeed;
@@ -201,21 +201,21 @@
 void updateMotorsToFollowLineFast()
 {
     const int16_t drivingSpeed = 600;
-    const int followLineStrength = drivingSpeed * 5 / 4;
-    static int16_t lastPosition = 0;
+    const int32_t followLineStrength = drivingSpeed * 5 / 4;
+    static int16_t lastPosition = 1000;
 
     int16_t position = lineTracker.getLinePosition();
 
     int16_t speedLeft = drivingSpeed;
     int16_t speedRight = drivingSpeed;
-    int16_t reduction = (position - 1000) * followLineStrength / 1000;
-    if(reduction < 0)
+    int32_t veer = (position - 1000) * followLineStrength / 1000 + (position - lastPosition) * 200;
+    if(veer > 0)
     {
-        speedLeft = reduceSpeed(speedLeft, -reduction);   
+        speedRight = reduceSpeed(speedRight, veer);
     }
     else
     {
-        speedRight = reduceSpeed(speedRight, reduction);
+        speedLeft = reduceSpeed(speedLeft, -veer);   
     }
     
     motorsSpeedSet(speedLeft, speedRight);
@@ -228,11 +228,20 @@
     Pacer reportPacer(200000);
     
     loadCalibration();
+    uint32_t loopCount = 0;
+    Timer timer;
+    timer.start();
     while(1)
     {
+        loopCount += 1;
         updateReckonerFromEncoders();
         loggerService();
-
+        
+        if ((loopCount % 256) == 0)
+        {
+            pc.printf("%d\r\n", lineTracker.getLinePosition());
+        }
+        
         lineTracker.read();
         updateMotorsToFollowLineFast();        
     }