Diff: main.cpp

Revision:

18:b65fbb795396

Parent:

17:2df9861f53ee

Child:

19:a11ffc903774

--- a/main.cpp	Mon Feb 24 01:38:55 2014 +0000
+++ b/main.cpp	Mon Feb 24 02:32:59 2014 +0000
@@ -32,15 +32,6 @@
     }
 }
 
-void __attribute__((noreturn)) driveHome()
-{
-    led1 = 1; led2 = 1; led3 = 0; led4 = 0;
-    while(1)
-    {
-        
-    }
-}
-
 void updateReckonerFromEncoders()
 {
     while(encoderBuffer.hasEvents())
@@ -64,3 +55,49 @@
     }
 }
 
+// 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 det()
+{
+    // TODO: get rid of the magic numbers here (i.e. 30)
+    float s = (float)reckoner.sin / (1 << 30);
+    float c = (float)reckoner.cos / (1 << 30);
+    return reckoner.x * s - reckoner.y * c;
+}
+
+void __attribute__((noreturn)) driveHome()
+{
+    led1 = 1; led2 = 1; led3 = 0; led4 = 0;
+    
+    // First, point the robot at the goal.
+    bool dir = false;
+    uint16_t transitions = 0;
+    Timer timer;
+    timer.start();
+    while(transitions < 100 || timer.read_ms() < 500)
+    {
+        updateReckonerFromEncoders();
+        {
+            bool nextDir = det() > 0;
+            if (nextDir != dir) { transitions++; }
+            dir = nextDir;
+        }
+        
+        if(dir)
+        {
+            led3 = 1;
+            motorsSpeedSet(-300, 300);   
+        }
+        else
+        {
+            led3 = 0;
+            motorsSpeedSet(300, -300);
+        }
+    }
+    motorsSpeedSet(0, 0);
+    
+    while(1)
+    {
+        
+    }
+}