Diff: main.cpp

Revision:

20:dbec34f0e76b

Parent:

19:a11ffc903774

Child:

21:c279c6a83671

--- a/main.cpp	Tue Feb 25 02:58:16 2014 +0000
+++ b/main.cpp	Thu Feb 27 19:46:35 2014 +0000
@@ -25,7 +25,8 @@
     //testLineSensors();
     //testReckoner();
     //testButtons();
-    testDriveHome();
+    //testDriveHome();
+    testFinalSettleIn();
 
     while(1)
     {
@@ -61,6 +62,15 @@
     return sqrt((float)reckoner.x * reckoner.x + (float)reckoner.y * reckoner.y);   
 }
 
+float dotProduct()
+{
+    float s = (float)reckoner.sin / (1 << 30);
+    float c = (float)reckoner.cos / (1 << 30);
+    float magn = magnitude();
+    if (magn == 0){ return 0; }    
+    return ((float)reckoner.x * c + (float)reckoner.y * s) / magn;
+}
+
 // The closer this is to zero, the closer we are to pointing towards the home position.
 // It is basically a cross product of the two vectors (x, y) and (cos, sin).
 float determinant()
@@ -83,11 +93,19 @@
     }
 }
 
-void __attribute__((noreturn)) driveHome()
+void driveHomeAlmost();
+void finalSettleIn();
+
+void driveHome()
+{
+    driveHomeAlmost();
+    finalSettleIn();
+}
+
+void driveHomeAlmost()
 {
     led1 = 1; led2 = 1; led3 = 0; led4 = 0;
     
-    // First, point the robot at the goal.
     bool dir = false;
     uint16_t transitions = 0;
     Timer timer;
@@ -135,22 +153,60 @@
         motorsSpeedSet(speedLeft, speedRight);
     }
     
-    //while(1)
-    {
-        
-    }
+    motorsSpeedSet(0, 0);
+}
+
+void finalSettleIn()
+{
+    led1 = 1; led2 = 1; led3 = 1; led4 = 0;
     
-    motorsSpeedSet(0, 0);    
-    Pacer reportPacer(200000);
+    const int16_t settleSpeed = 200;
+    const int16_t settleModificationStrength = 100;
+    
+    Timer timer;
+    timer.start();
+    
+    // State 0: rotating
+    // State 1: Trying to get into final orientation: want [cos, sin] == [1<<30, 0].
+    uint8_t state = 0;
+    
     while(1)
     {
-        if(reportPacer.pace())
+        updateReckonerFromEncoders();
+        
+        float dot = dotProduct();
+        int16_t speedModification = -dot * settleModificationStrength;
+        if (speedModification > settleModificationStrength)
+        {
+            speedModification = settleModificationStrength;    
+        }
+        else if (speedModification < -settleModificationStrength)
+        {
+            speedModification = -settleModificationStrength;
+        }
+        
+        if (state == 0 && timer.read_ms() >= 6000 && reckoner.cos > (1 << 29))
         {
-            led4 = 1;
-            pc.printf("%11d %11d %11d %11d | %11f\r\n",
-              reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
-              determinant());
-            led4 = 0;
-       }
+            led1 = 1; led2 = 0; led3 = 1; led4 = 0;
+            state = 1;
+        }
+        
+        int16_t rotationSpeed;
+        if (state == 1)
+        {
+            float s = (float)reckoner.sin / (1 << 30);
+            rotationSpeed = -s * 300;
+        }
+        else
+        {
+            rotationSpeed = settleSpeed;
+        }
+        
+        int16_t speedLeft = -rotationSpeed + speedModification;
+        int16_t speedRight = rotationSpeed + speedModification;
+        motorsSpeedSet(speedLeft, speedRight);
     }
+    
+    led1 = 1; led2 = 1; led3 = 1; led4 = 1;
+    motorsSpeedSet(0, 0);
 }