Diff: main.cpp

Revision:

19:a11ffc903774

Parent:

18:b65fbb795396

Child:

20:dbec34f0e76b

--- a/main.cpp	Mon Feb 24 02:32:59 2014 +0000
+++ b/main.cpp	Tue Feb 25 02:58:16 2014 +0000
@@ -1,5 +1,6 @@
 #include <mbed.h>
 #include <Pacer.h>
+#include <math.h>
 
 #include "motors.h"
 #include "encoders.h"
@@ -55,14 +56,31 @@
     }
 }
 
+float magnitude()
+{
+    return sqrt((float)reckoner.x * reckoner.x + (float)reckoner.y * reckoner.y);   
+}
+
 // 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()
+float determinant()
 {
     // 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;
+    return (reckoner.x * s - reckoner.y * c) / magnitude();
+}
+
+int16_t reduceSpeed(int16_t speed, int16_t reduction)
+{
+    if (reduction > speed)
+    {
+        return 0;   
+    }
+    else
+    {
+        return speed - reduction;
+    }
 }
 
 void __attribute__((noreturn)) driveHome()
@@ -74,30 +92,65 @@
     uint16_t transitions = 0;
     Timer timer;
     timer.start();
-    while(transitions < 100 || timer.read_ms() < 500)
+    
+    const int16_t maxSpeed = 300;
+    
+    while(1)
     {
         updateReckonerFromEncoders();
+        
+        float magn = magnitude();
+        
+        if (magn < (1<<17))  
         {
-            bool nextDir = det() > 0;
+            // We are within 8 encoder ticks, so go to the next step.
+            break;
+        }
+        
+        float det = determinant();
+        
+        {
+            bool nextDir = det > 0;
             if (nextDir != dir) { transitions++; }
             dir = nextDir;
         }
         
-        if(dir)
+        int16_t speedLeft = maxSpeed;
+        int16_t speedRight = maxSpeed;
+        if (magn < (1<<20)) // Within 64 encoder ticks of the origin, so slow down.
         {
-            led3 = 1;
-            motorsSpeedSet(-300, 300);   
+            int16_t reduction = (1 - magn/(1<<20)) * maxSpeed;
+            speedLeft = reduceSpeed(speedLeft, reduction);
+            speedRight = reduceSpeed(speedRight, reduction);
+        }
+        
+        if (det > 0)
+        {
+            speedLeft = reduceSpeed(speedLeft, det * 1000);
         }
         else
         {
-            led3 = 0;
-            motorsSpeedSet(300, -300);
+            speedRight = reduceSpeed(speedRight, -det * 1000);               
         }
+        motorsSpeedSet(speedLeft, speedRight);
     }
-    motorsSpeedSet(0, 0);
     
-    while(1)
+    //while(1)
     {
         
     }
+    
+    motorsSpeedSet(0, 0);    
+    Pacer reportPacer(200000);
+    while(1)
+    {
+        if(reportPacer.pace())
+        {
+            led4 = 1;
+            pc.printf("%11d %11d %11d %11d | %11f\r\n",
+              reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
+              determinant());
+            led4 = 0;
+       }
+    }
 }