From eebcaf4c92b22751136071ef55f28f9322d45d16 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E2=80=9CSRONG?= <“ougy.srong@ecam.fr”>
Date: Thu, 7 Nov 2024 20:13:35 +0100
Subject: [PATCH] autopilot_v3

---
 src/autopilot.cpp | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/autopilot.cpp b/src/autopilot.cpp
index 240d452..e78e0d0 100644
--- a/src/autopilot.cpp
+++ b/src/autopilot.cpp
@@ -85,8 +85,8 @@ int main(int argc, char **argv)
         float angular_speed = 0.0;
 
         // Thresholds for obstacle detection
-        float front_threshold = 0.7;
-        float side_threshold = 0.5;
+        float front_threshold = 0.65;
+        float side_threshold = 0.35;
 
         // Obstacle avoidance logic
         if (min_front < front_threshold)
@@ -104,7 +104,7 @@ int main(int argc, char **argv)
             if (left_clearance > side_threshold && left_clearance > right_clearance)
             {
                 // Turn left while moving forward slowly
-                vel_msg.linear.x = 0.05; // Slow forward speed
+                vel_msg.linear.x = 0.01; // Slow forward speed
                 angular_speed = max_angular_speed * (front_threshold - min_front) / front_threshold;
                 angular_speed = std::max(angular_speed, min_angular_speed);
                 vel_msg.angular.z = angular_speed;
@@ -113,7 +113,7 @@ int main(int argc, char **argv)
             else if (right_clearance > side_threshold && right_clearance > left_clearance)
             {
                 // Turn right while moving forward slowly
-                vel_msg.linear.x = 0.05; // Slow forward speed
+                vel_msg.linear.x = 0.01; // Slow forward speed
                 angular_speed = -max_angular_speed * (front_threshold - min_front) / front_threshold;
                 angular_speed = std::min(angular_speed, -min_angular_speed);
                 vel_msg.angular.z = angular_speed;
@@ -122,7 +122,7 @@ int main(int argc, char **argv)
             else if (left_clearance > side_threshold)
             {
                 // Turn left even if right is blocked
-                vel_msg.linear.x = 0.05; // Slow forward speed
+                vel_msg.linear.x = 0.01; // Slow forward speed
                 angular_speed = max_angular_speed * (front_threshold - min_front) / front_threshold;
                 angular_speed = std::max(angular_speed, min_angular_speed);
                 vel_msg.angular.z = angular_speed;
@@ -131,7 +131,7 @@ int main(int argc, char **argv)
             else if (right_clearance > side_threshold)
             {
                 // Turn right even if left is blocked
-                vel_msg.linear.x = 0.05; // Slow forward speed
+                vel_msg.linear.x = 0.01; // Slow forward speed
                 angular_speed = -max_angular_speed * (front_threshold - min_front) / front_threshold;
                 angular_speed = std::min(angular_speed, -min_angular_speed);
                 vel_msg.angular.z = angular_speed;
@@ -140,8 +140,8 @@ int main(int argc, char **argv)
             else
             {
                 // Both sides are blocked; perform escape maneuver
-                vel_msg.linear.x = -0.05; // Move backward slowly
-                vel_msg.angular.z = 0.5;  // Rotate to try to find a clear path
+                vel_msg.linear.x = -0.05; 
+                vel_msg.angular.z = 1.0;  // Rotate to try to find a clear path
                 ROS_WARN("No clear path ahead; moving backward and rotating.");
             }
         }