David Grayson / Mbed 2 deprecated DeadReckoning

David's dead reckoning code for the LVBots competition on March 6th. Uses the mbed LPC1768, DRV8835, QTR-3RC, and two DC motors with encoders.

Dependencies:   PololuEncoder Pacer mbed GeneralDebouncer

Diff: reckoner.cpp

Revision:
36:ccb03b734737
Parent:
21:c279c6a83671
Child:
37:23000a47ed2b
--- a/reckoner.cpp	Wed Mar 05 04:43:36 2014 +0000
+++ b/reckoner.cpp	Thu Mar 06 02:39:07 2014 +0000
@@ -153,12 +153,18 @@
 
 void Reckoner::handleRight()
 {
-    cos += ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE;
-    sin -= ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE;
+    handleTurnRadians(-DA);
 }
 
 void Reckoner::handleLeft()
 {
-    cos -= ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE;
-    sin += ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE;
+    handleTurnRadians(DA);
+}
+
+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;
+    cos += dc;
+    sin += ds;
 }
\ No newline at end of file