From f4373cdc590416170902193aac92b3428dedf972 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Wed, 4 Feb 2026 14:50:41 -0500
Subject: [PATCH] impl CheckReachedDestination

---
 src/robot-nav.cpp | 6 ++++--
 src/robot.h       | 3 +++
 2 files changed, 7 insertions(+), 2 deletions(-)

diff --git a/src/robot-nav.cpp b/src/robot-nav.cpp
index 51405ba..e975cea 100644
--- a/src/robot-nav.cpp
+++ b/src/robot-nav.cpp
@@ -44,12 +44,14 @@ void Robot::SetDestination(const Pose& dest)
 
 bool Robot::CheckReachedDestination(void)
 {
-    bool retVal = false;
     /**
      * TODO: Add code to check if you've reached destination here.
      */
 
-    return retVal;
+    float sq_dist =  pow((destPose.x - currPose.x),2) + pow((destPose.y-currPose.y),2);
+    float rad_error = abs(destPose.theta - currPose.theta);
+
+    return sq_dist < TARGET_SQ_DIST && rad_error < TARGET_RAD_ERR;
 }
 
 void Robot::DriveToPoint(void)
diff --git a/src/robot.h b/src/robot.h
index 4193edc..0c043aa 100644
--- a/src/robot.h
+++ b/src/robot.h
@@ -2,6 +2,9 @@
 
 #include "chassis.h"
 
+const float TARGET_SQ_DIST = pow(3.0, 2); // distance from setpoint that is ok (cm)
+const float TARGET_RAD_ERR = 0.25; // angle from setpoint that is ok (rad)
+
 class Robot
 {
 protected: