Diff: Odometry/Odometry2.cpp

Revision:

72:b2a128486332

Parent:

71:95d76c181b22

Child:

79:d97090bb6470

--- a/Odometry/Odometry2.cpp	Fri Apr 10 18:01:54 2015 +0000
+++ b/Odometry/Odometry2.cpp	Fri Apr 10 18:40:58 2015 +0000
@@ -6,6 +6,7 @@
  */
 
 #include "Odometry2.h"
+#include "defines.h"
 
 extern Serial logger;
 
@@ -16,8 +17,8 @@
     m_qei_right = qei_right;
     m_v = v;
     
-    m_distPerTick_left = radius_left/qei_left->getPulsesPerRev()*2*3.1415;
-    m_distPerTick_right = radius_right/qei_right->getPulsesPerRev()*2*3.1415;
+    m_distPerTick_left = radius_left/qei_left->getPulsesPerRev()*2*PI;
+    m_distPerTick_right = radius_right/qei_right->getPulsesPerRev()*2*PI;
     
     m_pulses_left = qei_left->getPulses();
     m_pulses_right = qei_right->getPulses();
@@ -68,6 +69,9 @@
     x += dx;
     y += dy;
     theta += deltaTheta;
+    
+    while(theta > PI) theta -= 2*PI;
+    while(theta <= -PI) theta += 2*PI;
 }