From 324a51cf4c1220ddb7aa8831334bc49eceed0480 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Wed, 4 Feb 2026 22:49:13 -0500
Subject: [PATCH] define tau

---
 src/robot-nav.cpp | 16 +++++++---------
 1 file changed, 7 insertions(+), 9 deletions(-)

diff --git a/src/robot-nav.cpp b/src/robot-nav.cpp
index d1a0ec0..b9663d0 100644
--- a/src/robot-nav.cpp
+++ b/src/robot-nav.cpp
@@ -46,17 +46,17 @@ float Robot::SquareDistance(void) {
     return pow((destPose.x - currPose.x),2) + pow((destPose.y-currPose.y),2);
 }
 
-float Robot::RadError(float current, float destination) {
+#define TAU (float) TWO_PI
 
+float Robot::RadError(float current, float destination) {
     float diff = destination-current;
-    diff += PI_2;
-    diff %= TAU;
-    diff -= PI_2;
+    diff += HALF_PI;
+    diff = fmod(diff,TAU); // floating point modulo
+    diff -= HALF_PI;
 
     if (diff > 180) {
         diff -= TAU;
     }
-
     return diff;
 }
 
@@ -66,8 +66,6 @@ bool Robot::CheckReachedDestination(void)
      * TODO: Add code to check if you've reached destination here.
      */
 
-    float rad_error = abs(destPose.theta - currPose.theta);
-
     return SquareDistance() < TARGET_SQ_DIST 
         && RadError(currPose.theta, destPose.theta) < TARGET_RAD_ERR;
 }
@@ -84,8 +82,8 @@ void Robot::DriveToPoint(void)
             target_heading = atan2(destPose.x - currPose.x, destPose.y - currPose.y);
         }
 
-        float heading_err = destPose.theta - target_heading;
-        float forward_effort = cos(heading_err);
+        float heading_err = RadError(currPose.theta, target_heading);
+        float forward_effort = cos(heading_err) * sqrt(SquareDistance());
 
 
         /**