Diff: main.cpp

Revision:

27:2456f68be679

Parent:

26:7e7c376a7446

Child:

28:4374035df5e0

--- a/main.cpp	Fri Feb 28 01:40:39 2014 +0000
+++ b/main.cpp	Sat Mar 01 01:46:35 2014 +0000
@@ -53,8 +53,8 @@
     loadCalibrationAndFindLine();
     //findLineAndCalibrate();
     
-    setLeds(1, 1, 0, 0);
-    turnRightToFindLine();
+    //setLeds(1, 1, 0, 0);
+    //turnRightToFindLine();
     setLeds(0, 0, 1, 0);
     followLineToEnd();
     setLeds(1, 0, 1, 0);
@@ -273,11 +273,11 @@
         int16_t reduction = (lineTracker.getLinePosition() - 1500) * followLineStrength / 1500;
         if(reduction < 0)
         {
-            reduceSpeed(speedLeft, -reduction);   
+            speedLeft = reduceSpeed(speedLeft, -reduction);   
         }
         else
         {
-            reduceSpeed(speedRight, reduction);
+            speedRight = reduceSpeed(speedRight, reduction);
         }
         
         
@@ -329,8 +329,8 @@
 
 void finalSettleIn()
 {
-    const int16_t settleSpeed = 200;
-    const int16_t settleModificationStrength = 100;
+    const int16_t settleSpeed = 300;
+    const int16_t settleModificationStrength = 150;
     
     Timer timer;
     timer.start();
@@ -339,10 +339,17 @@
     // State 1: Trying to get into final orientation: want [cos, sin] == [1<<30, 0].
     uint8_t state = 0;
     
-    Pacer reportPacer(200000);   
+    Pacer reportPacer(200000);
+    Pacer motorUpdatePacer(10000);   
 
+    float integral = 0;
+    
+    motorsSpeedSet(-settleSpeed, settleSpeed); // avoid waiting 10 ms before we start settling
+    
     while(1)
     {
+        led1 = (state == 1);
+        
         updateReckonerFromEncoders();
         
         float dot = dotProduct();
@@ -368,20 +375,33 @@
             break;   
         }
         
-        int16_t rotationSpeed;
-        if (state == 1)
+        if (motorUpdatePacer.pace())
         {
-            float s = (float)reckoner.sin / (1 << 30);
-            rotationSpeed = -s * 600;
+            int16_t rotationSpeed;
+            if (state == 1)
+            {
+                float s = (float)reckoner.sin / (1 << 30);
+                integral += s;
+                rotationSpeed = -(s * 2400 + integral * 20);
+                
+                if (rotationSpeed > 300)
+                {
+                    rotationSpeed = 300; 
+                }
+                if (rotationSpeed < -300)
+                {
+                    rotationSpeed = -300; 
+                }
+            }
+            else
+            {
+                rotationSpeed = settleSpeed;
+            }
+            
+            int16_t speedLeft = -rotationSpeed + speedModification;
+            int16_t speedRight = rotationSpeed + speedModification;
+            motorsSpeedSet(speedLeft, speedRight);
         }
-        else
-        {
-            rotationSpeed = settleSpeed;
-        }
-        
-        int16_t speedLeft = -rotationSpeed + speedModification;
-        int16_t speedRight = rotationSpeed + speedModification;
-        motorsSpeedSet(speedLeft, speedRight);
         
         if (state == 1 && reportPacer.pace())
         {