Diff: reckoner.cpp

Revision:

12:835a4d24ae3b

Child:

13:bba5b3abd13f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/reckoner.cpp	Sun Feb 23 22:23:34 2014 +0000
@@ -0,0 +1,77 @@
+#include <mbed.h>
+#include "reckoner.h"
+
+/**
+W: Wheel-to-wheel distance:              150 mm  // TODO: correct this
+G: Gear ratio factor:                    30
+T: Encoder ticks per backshaft rotation: 12       (three-toothed encoder wheel)
+R: Wheel radius:                         70 mm
+
+Dw: Distance wheel turns per encoder tick = 2*pi*R/(G*T)
+Df: Distance robot moves forward per encoder tick = Dw/2
+dA: Change in angle per encoder tick = Dw/W = 2*pi*70/(30*12) / 150 = 0.00814486984
+**/
+
+#define LOG_UNIT_MAGNITUDE 30
+
+#define DA 8745487  // 0.00814486984 * 0x40000000
+
+#define LOG_COS_TO_X_CONVERSION  14    // 30 - 16
+
+Reckoner reckoner;
+
+Reckoner::Reckoner()
+{
+  cos = 1 << LOG_UNIT_MAGNITUDE;
+  sin = 0;
+  x = 0;
+  y = 0;
+}
+    
+void Reckoner::handleTickLeftForward()
+{
+    handleForward();
+    handleRight();
+}
+
+void Reckoner::handleTickLeftBackward()
+{
+    handleBackward();
+    handleLeft();   
+}
+
+void Reckoner::handleTickRightForward()
+{
+    handleForward();
+    handleLeft();   
+}
+
+void Reckoner::handleTickRightBackward()
+{
+    handleBackward();
+    handleRight();   
+}
+
+void Reckoner::handleForward()
+{
+    x += cos >> LOG_COS_TO_X_CONVERSION;
+    y += sin >> LOG_COS_TO_X_CONVERSION;
+}
+
+void Reckoner::handleBackward()
+{
+    x -= cos >> LOG_COS_TO_X_CONVERSION;
+    y -= sin >> LOG_COS_TO_X_CONVERSION;
+}
+
+void Reckoner::handleRight()
+{
+    cos += ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE;
+    sin -= ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE;
+}
+
+void Reckoner::handleLeft()
+{
+    cos -= ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE;
+    sin += ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE;
+}
\ No newline at end of file