From 81ebd8b7a276800d03a6b4bfc1a65172e7a45c8c Mon Sep 17 00:00:00 2001
From: Pimpest <82343504+pimpest@users.noreply.github.com>
Date: Wed, 15 Apr 2026 13:55:16 +0200
Subject: [PATCH] Modified rotate_acorns precision and decel

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

diff --git a/toid_behaviors/src/rotate_acorns.cpp b/toid_behaviors/src/rotate_acorns.cpp
index ffbf905..044d574 100644
--- a/toid_behaviors/src/rotate_acorns.cpp
+++ b/toid_behaviors/src/rotate_acorns.cpp
@@ -78,8 +78,8 @@ void RotateAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
   yaw += M_PI/2;
 
 
-  pose_global.pose.position.x += std::cos(yaw) * 0.0 - std::sin(yaw) * + 0.005;
-  pose_global.pose.position.y += std::sin(yaw) * 0.0 + std::cos(yaw) * + 0.005;
+  pose_global.pose.position.x += std::cos(yaw) * 0.0 - std::sin(yaw) * - 0.005;
+  pose_global.pose.position.y += std::sin(yaw) * 0.0 + std::cos(yaw) * - 0.005;
   tf2::convert(tf2::Quaternion::createFromRPY(0,0,yaw ), pose_global.pose.orientation);
 
   yaw = angles::normalize_angle(yaw);
@@ -143,7 +143,7 @@ double RotateAcorns::calc_speed(
   const double upper_vel_ = angular_speed + max_angular_accel_ * control_duration_;
   const double lower_vel_ = angular_speed - max_angular_accel_ * control_duration_;
 
-  const double speed_until_overshoot = std::sqrt(2.0 * max_angular_accel_ * std::fabs(err));
+  const double speed_until_overshoot = std::sqrt(2.0 * max_angular_decel_ * std::fabs(err));
 
   const double requested_speed = sign * std::min(speed_until_overshoot, max_angular_speed_);
   const double speed = std::clamp(requested_speed, lower_vel_, upper_vel_);
@@ -171,7 +171,7 @@ ResultStatus RotateAcorns::updateVel(
     speed = calc_speed(err, sign, angular_speed);
   }
 
-  if (min_turn_angle_ == 0 && std::fabs(current_yaw - target_angle_) < 0.01) {
+  if (min_turn_angle_ == 0 && std::fabs(current_yaw - target_angle_) < 0.004) {
     return ResultStatus{Status::SUCCEEDED};
   }