From 2619a9b0b513f900211607e9fb2f196e31065a9b Mon Sep 17 00:00:00 2001
From: Pimpest <82343504+pimpest@users.noreply.github.com>
Date: Wed, 15 Apr 2026 13:23:31 +0200
Subject: [PATCH] Fix target update distance

---
 toid_behaviors/src/approach_acorns.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/toid_behaviors/src/approach_acorns.cpp b/toid_behaviors/src/approach_acorns.cpp
index 270cb9f..f8ce5e9 100644
--- a/toid_behaviors/src/approach_acorns.cpp
+++ b/toid_behaviors/src/approach_acorns.cpp
@@ -98,7 +98,7 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
   }
 
   // Smooth out approach
-  if (x * x + y * y < 0.42) {
+  if (x * x + y * y < 0.42 * 0.42) {
     return;
   }
 
@@ -275,7 +275,7 @@ ResultStatus ApproachAcorns::updateVel(
     return ResultStatus{Status::RUNNING};
   }
 
-  if (dist_left <= 0.02) {
+  if (dist_left >= 0.001 && dist_left <= 0.02) {
     out_vel.linear.x = velocityTarget(dist_left);
     out_vel.angular.z = std::clamp(kp_ * angle_dist, -max_angular_speed_, max_angular_speed_);
     last_speed_ = out_vel.linear.x;