diff --git a/src/robot-nav.cpp b/src/robot-nav.cpp
index 7146f21..d1a0ec0 100644
--- a/src/robot-nav.cpp
+++ b/src/robot-nav.cpp
@@ -46,6 +46,20 @@ float Robot::SquareDistance(void) {
     return pow((destPose.x - currPose.x),2) + pow((destPose.y-currPose.y),2);
 }
 
+float Robot::RadError(float current, float destination) {
+
+    float diff = destination-current;
+    diff += PI_2;
+    diff %= TAU;
+    diff -= PI_2;
+
+    if (diff > 180) {
+        diff -= TAU;
+    }
+
+    return diff;
+}
+
 bool Robot::CheckReachedDestination(void)
 {
     /**
@@ -54,7 +68,8 @@ bool Robot::CheckReachedDestination(void)
 
     float rad_error = abs(destPose.theta - currPose.theta);
 
-    return SquareDistance() < TARGET_SQ_DIST && rad_error < TARGET_RAD_ERR;
+    return SquareDistance() < TARGET_SQ_DIST 
+        && RadError(currPose.theta, destPose.theta) < TARGET_RAD_ERR;
 }
 
 void Robot::DriveToPoint(void)
diff --git a/src/robot.h b/src/robot.h
index 7d8bd5e..8ddd65a 100644
--- a/src/robot.h
+++ b/src/robot.h
@@ -55,4 +55,7 @@ protected:
 
     // Square distance between current and target pose
     float SquareDistance(void);
+
+    // minimum radians to transform current to setpoint 
+    float RadError(float current, float setpoint);
 };