Revision 20:dbec34f0e76b, committed 2014-02-27

Comitter:

DavidEGrayson

Date:

Thu Feb 27 19:46:35 2014 +0000

Parent:

19:a11ffc903774

Child:

21:c279c6a83671

Commit message:

Broke up driveHome into two functions.

Changed in this revision

--- 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);
 }

--- a/test.cpp	Tue Feb 25 02:58:16 2014 +0000
+++ b/test.cpp	Thu Feb 27 19:46:35 2014 +0000
@@ -12,7 +12,7 @@
 #include "reckoner.h"
 #include "buttons.h"
 
-void __attribute__((noreturn)) driveHome();
+void __attribute__((noreturn)) infiniteReckonerReportLoop();
 void printBar(const char * name, uint16_t adcResult);
 
 void testDriveHome()
@@ -23,6 +23,18 @@
         updateReckonerFromEncoders();
     }   
     driveHome();
+    infiniteReckonerReportLoop();     
+}
+
+void testFinalSettleIn()
+{
+    led1 = 1;
+    while(!button1DefinitelyPressed())
+    {
+        updateReckonerFromEncoders();
+    }   
+    finalSettleIn();
+    infiniteReckonerReportLoop();     
 }
 
 
@@ -147,6 +159,23 @@
     }
 }
 
+void infiniteReckonerReportLoop()
+{
+    Pacer reportPacer(200000);
+    while(1)
+    {
+        if(reportPacer.pace())
+        {
+            led4 = 1;
+            pc.printf("%11d %11d %11d %11d | %11f %11f\r\n",
+              reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
+              determinant(), dotProduct());
+            led4 = 0;
+       }
+    }
+   
+}
+
 void printBar(const char * name, uint16_t adcResult)
 {
     pc.printf("%-2s %5d |", name, adcResult);

--- a/test.h	Tue Feb 25 02:58:16 2014 +0000
+++ b/test.h	Thu Feb 27 19:46:35 2014 +0000
@@ -6,9 +6,15 @@
 void __attribute__((noreturn)) testReckoner();
 void __attribute__((noreturn)) testButtons();
 void __attribute__((noreturn)) testDriveHome();
+void __attribute__((noreturn)) testFinalSettleIn();
 
 // These are actually defined in main.h:
 
 void updateReckonerFromEncoders();
-void __attribute__((noreturn)) driveHome();
-float determinant();
\ No newline at end of file
+
+void driveHome();
+void driveHomeAlmost();
+void finalSettleIn();
+
+float determinant();
+float dotProduct();
\ No newline at end of file