Revision 27:2456f68be679, committed 2014-03-01

Comitter:

DavidEGrayson

Date:

Sat Mar 01 01:46:35 2014 +0000

Parent:

26:7e7c376a7446

Child:

28:4374035df5e0

Commit message:

Fixed a major bug in the line following (reduceSpeed return value was not used). Made finalSettleIn better by adding an integral term and increasing the settleSpeed from 200 to 300.

Changed in this revision

--- 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())
         {