Diff: reckoner.cpp

Revision:

46:f11cb4f93aac

Parent:

37:23000a47ed2b

--- a/reckoner.cpp	Wed Apr 15 19:19:19 2015 +0000
+++ b/reckoner.cpp	Wed Apr 15 21:19:22 2015 +0000
@@ -1,5 +1,6 @@
 #include <mbed.h>
 #include "reckoner.h"
+#include "math.h"
 
 /**
 First, we define two int32_t variables cos and sin that make a unit vector.
@@ -7,7 +8,7 @@
 We need to choose units for them that allow for a lot of accuracy without overflowing.
 We choose the units to be 1/(1 << 30) so that they will typically be about half of the
 way between 0 and overflow.
-The starting value ofvalue of the [cos, sin] will be [1 << 30, 0].
+The starting value of the [cos, sin] will be [1 << 30, 0].
 To record this choice, we define LOG_UNIT_MAGNITUDE:
 **/
 #define LOG_UNIT_MAGNITUDE 30
@@ -109,8 +110,8 @@
 
 void Reckoner::reset()
 {
-  cos = 1 << LOG_UNIT_MAGNITUDE;
-  sin = 0;
+  cosv = 1 << LOG_UNIT_MAGNITUDE;
+  sinv = 0;
   x = 0;
   y = 0;
 }
@@ -141,14 +142,14 @@
 
 void Reckoner::handleForward()
 {
-    x += cos >> LOG_COS_TO_X_CONVERSION;
-    y += sin >> LOG_COS_TO_X_CONVERSION;
+    x += cosv >> LOG_COS_TO_X_CONVERSION;
+    y += sinv >> LOG_COS_TO_X_CONVERSION;
 }
 
 void Reckoner::handleBackward()
 {
-    x -= cos >> LOG_COS_TO_X_CONVERSION;
-    y -= sin >> LOG_COS_TO_X_CONVERSION;
+    x -= cosv >> LOG_COS_TO_X_CONVERSION;
+    y -= sinv >> LOG_COS_TO_X_CONVERSION;
 }
 
 void Reckoner::handleRight()
@@ -165,10 +166,18 @@
 
 void Reckoner::handleTurnRadians(int32_t radians)
 {
-    int32_t dc = -((int64_t)sin * radians) >> LOG_UNIT_MAGNITUDE;
-    int32_t ds = ((int64_t)cos * radians) >> LOG_UNIT_MAGNITUDE;
-    dc = -((int64_t)(sin+ds/2) * radians) >> LOG_UNIT_MAGNITUDE;
-    ds = ((int64_t)(cos+dc/2) * radians) >> LOG_UNIT_MAGNITUDE;
-    cos += dc;
-    sin += ds;
+    int32_t dc = -((int64_t)sinv * radians) >> LOG_UNIT_MAGNITUDE;
+    int32_t ds = ((int64_t)cosv * radians) >> LOG_UNIT_MAGNITUDE;
+    dc = -((int64_t)(sinv+ds/2) * radians) >> LOG_UNIT_MAGNITUDE;
+    ds = ((int64_t)(cosv+dc/2) * radians) >> LOG_UNIT_MAGNITUDE;
+    cosv += dc;
+    sinv += ds;
+}
+
+// For angle, 0x20000000 represents 45 degrees.
+void Reckoner::setTurnAngle(int32_t angle)
+{
+    double angleToRadiansFactor = 1.46291808e-9;  // pi/4 / 0x20000000
+    cosv = (int32_t)(cos(angle * angleToRadiansFactor) * (1 << LOG_UNIT_MAGNITUDE));
+    sinv = (int32_t)(sin(angle * angleToRadiansFactor) * (1 << LOG_UNIT_MAGNITUDE));
 }
\ No newline at end of file