diff --git a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp
index 90f4779..e4428ef 100644
--- a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp
+++ b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp
@@ -25,6 +25,8 @@
 #include <modelec_interfaces/srv/odometry_set_pid.hpp>
 #include <modelec_interfaces/srv/odometry_add_waypoint.hpp>
 
+#include <std_msgs/msg/bool.hpp>
+
 namespace Modelec
 {
     class PCBOdoInterface : public rclcpp::Node
@@ -67,9 +69,9 @@ namespace Modelec
         rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr odo_set_pos_subscriber_;
         rclcpp::Subscription<modelec_interfaces::msg::OdometryPid>::SharedPtr odo_set_pid_subscriber_;
 
-        void AddWaypointCallback(const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) const;
-        void SetPosCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) const;
-        void SetPIDCallback(const modelec_interfaces::msg::OdometryPid::SharedPtr msg) const;
+        void AddWaypointCallback(const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg);
+        void SetPosCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
+        void SetPIDCallback(const modelec_interfaces::msg::OdometryPid::SharedPtr msg);
 
         rclcpp::Service<modelec_interfaces::srv::OdometryToF>::SharedPtr get_tof_service_;
         rclcpp::Service<modelec_interfaces::srv::OdometrySpeed>::SharedPtr get_speed_service_;
@@ -79,6 +81,9 @@ namespace Modelec
         rclcpp::Service<modelec_interfaces::srv::OdometrySetPid>::SharedPtr set_pid_service_;
         rclcpp::Service<modelec_interfaces::srv::OdometryAddWaypoint>::SharedPtr add_waypoint_service_;
 
+        rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr start_odo_sub_;
+        bool start_odo_ = false;
+
         // Promises and mutexes to synchronize service responses asynchronously
         std::queue<std::promise<long>> tof_promises_;
         std::mutex tof_mutex_;
@@ -138,28 +143,28 @@ namespace Modelec
         bool isOk = false;
 
     public:
-        void SendToPCB(const std::string& data) const;
+        void SendToPCB(const std::string& data);
         void SendToPCB(const std::string& order, const std::string& elem,
-                       const std::vector<std::string>& data = {}) const;
+                       const std::vector<std::string>& data = {});
 
-        void GetData(const std::string& elem, const std::vector<std::string>& data = {}) const;
-        void SendOrder(const std::string& elem, const std::vector<std::string>& data = {}) const;
+        void GetData(const std::string& elem, const std::vector<std::string>& data = {});
+        void SendOrder(const std::string& elem, const std::vector<std::string>& data = {});
 
-        void GetPos() const;
-        void GetSpeed() const;
-        void GetToF(const int& tof) const;
+        void GetPos();
+        void GetSpeed();
+        void GetToF(const int& tof);
 
-        void SetRobotPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) const;
-        void SetRobotPos(long x, long y, double theta) const;
+        void SetRobotPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
+        void SetRobotPos(long x, long y, double theta);
 
-        void AddWaypoint(modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) const;
-        void AddWaypoint(int index, bool IsStopPoint, long x, long y, double theta) const;
+        void AddWaypoint(modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg);
+        void AddWaypoint(int index, bool IsStopPoint, long x, long y, double theta);
 
-        void SetStart(const modelec_interfaces::msg::OdometryStart::SharedPtr msg) const;
-        void SetStart(bool start) const;
+        void SetStart(const modelec_interfaces::msg::OdometryStart::SharedPtr msg);
+        void SetStart(bool start);
 
-        void GetPID() const;
-        void SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg) const;
-        void SetPID(float p, float i, float d) const;
+        void GetPID();
+        void SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg);
+        void SetPID(float p, float i, float d);
     };
 } // namespace Modelec
diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp
index 6a6e22f..2e62925 100644
--- a/src/modelec_com/src/pcb_odo_interface.cpp
+++ b/src/modelec_com/src/pcb_odo_interface.cpp
@@ -170,6 +170,14 @@ namespace Modelec
             {
                 HandleAddWaypoint(request, response);
             });
+
+        start_odo_sub_ = this->create_subscription<std_msgs::msg::Bool>(
+            "odometry/start", 10,
+            [this](const std_msgs::msg::Bool::SharedPtr msg)
+            {
+                start_odo_ = msg->data;
+                SendOrder("START", {std::to_string(msg->data)});
+            });
     }
 
     PCBOdoInterface::~PCBOdoInterface()
@@ -313,17 +321,17 @@ namespace Modelec
         }
     }
 
-    void PCBOdoInterface::AddWaypointCallback(const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) const
+    void PCBOdoInterface::AddWaypointCallback(const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg)
     {
         AddWaypoint(msg);
     }
 
-    void PCBOdoInterface::SetPosCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) const
+    void PCBOdoInterface::SetPosCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
     {
         SetRobotPos(msg);
     }
 
-    void PCBOdoInterface::SetPIDCallback(const modelec_interfaces::msg::OdometryPid::SharedPtr msg) const
+    void PCBOdoInterface::SetPIDCallback(const modelec_interfaces::msg::OdometryPid::SharedPtr msg)
     {
         SetPID(msg);
     }
@@ -603,7 +611,7 @@ namespace Modelec
         }
     }
 
-    void PCBOdoInterface::SendToPCB(const std::string& data) const
+    void PCBOdoInterface::SendToPCB(const std::string& data)
     {
         if (pcb_publisher_)
         {
@@ -614,7 +622,7 @@ namespace Modelec
     }
 
     void PCBOdoInterface::SendToPCB(const std::string& order, const std::string& elem,
-                                    const std::vector<std::string>& data) const
+                                    const std::vector<std::string>& data)
     {
         std::string command = order + ";" + elem;
         for (const auto& d : data)
@@ -626,37 +634,37 @@ namespace Modelec
         SendToPCB(command);
     }
 
-    void PCBOdoInterface::GetData(const std::string& elem, const std::vector<std::string>& data) const
+    void PCBOdoInterface::GetData(const std::string& elem, const std::vector<std::string>& data)
     {
         SendToPCB("GET", elem, data);
     }
 
-    void PCBOdoInterface::SendOrder(const std::string& elem, const std::vector<std::string>& data) const
+    void PCBOdoInterface::SendOrder(const std::string& elem, const std::vector<std::string>& data)
     {
         SendToPCB("SET", elem, data);
     }
 
-    void PCBOdoInterface::GetPos() const
+    void PCBOdoInterface::GetPos()
     {
         GetData("POS");
     }
 
-    void PCBOdoInterface::GetSpeed() const
+    void PCBOdoInterface::GetSpeed()
     {
         GetData("SPEED");
     }
 
-    void PCBOdoInterface::GetToF(const int& tof) const
+    void PCBOdoInterface::GetToF(const int& tof)
     {
         GetData("DIST", {std::to_string(tof)});
     }
 
-    void PCBOdoInterface::SetRobotPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) const
+    void PCBOdoInterface::SetRobotPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
     {
         SetRobotPos(msg->x, msg->y, msg->theta);
     }
 
-    void PCBOdoInterface::SetRobotPos(const long x, const long y, const double theta) const
+    void PCBOdoInterface::SetRobotPos(const long x, const long y, const double theta)
     {
         std::vector<std::string> data = {
             std::to_string(x),
@@ -668,14 +676,20 @@ namespace Modelec
     }
 
     void PCBOdoInterface::AddWaypoint(
-        const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) const
+        const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg)
     {
         AddWaypoint(msg->id, msg->is_end, msg->x, msg->y, msg->theta);
     }
 
     void PCBOdoInterface::AddWaypoint(const int index, const bool IsStopPoint, const long x, const long y,
-                                      const double theta) const
+                                      const double theta)
     {
+        if (!start_odo_)
+        {
+            SendOrder("START", {std::to_string(true)});
+            start_odo_ = true;
+        }
+
         std::vector<std::string> data = {
             std::to_string(index),
             std::to_string(IsStopPoint),
@@ -687,27 +701,27 @@ namespace Modelec
         SendOrder("WAYPOINT", data);
     }
 
-    void PCBOdoInterface::SetStart(const modelec_interfaces::msg::OdometryStart::SharedPtr msg) const
+    void PCBOdoInterface::SetStart(const modelec_interfaces::msg::OdometryStart::SharedPtr msg)
     {
         SetStart(msg->start);
     }
 
-    void PCBOdoInterface::SetStart(bool start) const
+    void PCBOdoInterface::SetStart(bool start)
     {
         SendOrder("START", {std::to_string(start)});
     }
 
-    void PCBOdoInterface::GetPID() const
+    void PCBOdoInterface::GetPID()
     {
         GetData("PID");
     }
 
-    void PCBOdoInterface::SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg) const
+    void PCBOdoInterface::SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg)
     {
         SetPID(msg->p, msg->i, msg->d);
     }
 
-    void PCBOdoInterface::SetPID(float p, float i, float d) const
+    void PCBOdoInterface::SetPID(float p, float i, float d)
     {
         std::vector<std::string> data = {
             std::to_string(p),
diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml
index 4ee3757..55fcf48 100644
--- a/src/modelec_strat/data/config.xml
+++ b/src/modelec_strat/data/config.xml
@@ -10,6 +10,7 @@
             <refresh_rate>0.5</refresh_rate>
             <max_stationary_time_s>5</max_stationary_time_s>
             <min_emergency_distance_mm>500</min_emergency_distance_mm>
+            <margin_detection_table_mm>50</margin_detection_table_mm>
         </detection>
         <factor_close_enemy>-0.3</factor_close_enemy>
     </enemy>
@@ -55,16 +56,19 @@
             <pcb_odo>
                 <name>pcb_odo</name>
                 <port>/dev/USB_ODO</port>
+                <!--<port>/tmp/USB_ODO</port>-->
                 <baud_rate>115200</baud_rate>
             </pcb_odo>
             <pcb_alim>
                 <name>pcb_alim</name>
                 <port>/dev/USB_ALIM</port>
+                <!--<port>/tmp/USB_ALIM</port>-->
                 <baud_rate>115200</baud_rate>
             </pcb_alim>
             <pcb_action>
                 <name>pcb_action</name>
                 <port>/dev/USB_ACTION</port>
+                <!--<port>/tmp/USB_ACTION</port>-->
                 <baud_rate>115200</baud_rate>
             </pcb_action>
         </pcb>
diff --git a/src/modelec_strat/include/modelec_strat/enemy_manager.hpp b/src/modelec_strat/include/modelec_strat/enemy_manager.hpp
index 78c5417..5e3a5ce 100644
--- a/src/modelec_strat/include/modelec_strat/enemy_manager.hpp
+++ b/src/modelec_strat/include/modelec_strat/enemy_manager.hpp
@@ -44,6 +44,7 @@ namespace Modelec
         float max_stationary_time_s_ = 10.0f;
         float map_width_ = 0;
         float map_height_ = 0;
+        float margin_detection_table_ = 0.0f;
         float robot_width_ = 0;
         float robot_length_ = 0;
         double robot_radius_ = 0;
diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp
index 2c60451..7a6c49e 100644
--- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp
+++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp
@@ -8,6 +8,8 @@
 #include <modelec_interfaces/msg/spawn.hpp>
 #include <std_srvs/srv/empty.hpp>
 
+#include <std_msgs/msg/bool.hpp>
+
 #include "deposite_zone.hpp"
 #include "pathfinding.hpp"
 
@@ -95,8 +97,6 @@ namespace Modelec
 
         void SetSpawn(const std::string& name);
 
-        Point GetSpawnYellow() const;
-        Point GetSpawnBlue() const;
         Point GetSpawn() const;
 
     protected:
@@ -144,6 +144,8 @@ namespace Modelec
         rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr close_enemy_pos_sub_;
         rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_long_time_sub_;
 
+        rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr start_odo_pub_;
+
         modelec_interfaces::msg::OdometryPos last_enemy_pos_;
 
         bool await_rotate_ = false;
diff --git a/src/modelec_strat/include/modelec_strat/strat_fms.hpp b/src/modelec_strat/include/modelec_strat/strat_fms.hpp
index 668d326..da2594a 100644
--- a/src/modelec_strat/include/modelec_strat/strat_fms.hpp
+++ b/src/modelec_strat/include/modelec_strat/strat_fms.hpp
@@ -75,6 +75,6 @@ namespace Modelec
         rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr reset_strat_sub_;
         rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr tir_arm_sub_;
 
-        rclcpp::Client<modelec_interfaces::srv::OdometryStart>::SharedPtr client_start_;
+        rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr start_odo_pub_;
     };
 }
diff --git a/src/modelec_strat/src/enemy_manager.cpp b/src/modelec_strat/src/enemy_manager.cpp
index 55070df..1036a40 100644
--- a/src/modelec_strat/src/enemy_manager.cpp
+++ b/src/modelec_strat/src/enemy_manager.cpp
@@ -22,6 +22,8 @@ namespace Modelec
         map_width_ = Config::get<float>("config.map.size.map_width_mm", 3000.0);
         map_height_ = Config::get<float>("config.map.size.map_height_mm", 2000.0);
 
+        margin_detection_table_ = Config::get<float>("config.enemy.detection.margin_detection_table_mm", 50.0);
+
         robot_width_ = Config::get<float>("config.robot.size.width_mm", 500.0);
         robot_length_ = Config::get<float>("config.robot.size.length_mm", 500.0);
         robot_radius_ = std::max(robot_width_, robot_length_) / 2.0;
@@ -134,7 +136,7 @@ namespace Modelec
                 double y_global = robot_y + (x_local * std::sin(robot_theta) + y_local * std::cos(robot_theta));
 
                 // Check if in bounds
-                if (x_global >= 0 && x_global <= map_width_ && y_global >= 0 && y_global <= map_height_)
+                if (x_global >= 0 && x_global <= (map_width_ - margin_detection_table_) && y_global >= 0 && y_global <= (map_height_ - margin_detection_table_))
                 {
                     modelec_interfaces::msg::OdometryPos emergency_msg;
                     emergency_msg.x = x_global;
@@ -145,8 +147,7 @@ namespace Modelec
                     RCLCPP_WARN(this->get_logger(), "EMERGENCY CLOSE OBJECT DETECTED at x=%.2f y=%.2f (%.1f mm)", x_global, y_global, range_mm);
                 }
 
-                angle += msg->angle_increment;
-                continue;
+                break;;
             }
 
             // Convert to local robot frame
@@ -158,7 +159,7 @@ namespace Modelec
             double y_global = robot_y + (x_local * std::sin(robot_theta) + y_local * std::cos(robot_theta));
 
             // Ignore points outside of the table
-            if (x_global < 0 || x_global > map_width_ || y_global < 0 || y_global > map_height_)
+            if (x_global < 0 || x_global > (map_width_ - margin_detection_table_) || y_global < 0 || y_global > (map_height_ - margin_detection_table_))
             {
                 // RCLCPP_INFO(this->get_logger(), "Lidar point out of bounds: x=%.2f, y=%.2f", x_global, y_global);
 
diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp
index 54416de..61251f5 100644
--- a/src/modelec_strat/src/navigation_helper.cpp
+++ b/src/modelec_strat/src/navigation_helper.cpp
@@ -63,6 +63,8 @@ namespace Modelec
                 OnEnemyPositionLongTime(msg);
             });
 
+        start_odo_pub_ = node_->create_publisher<std_msgs::msg::Bool>("/odometry/start", 10);
+
         std::string deposite_zone_path = ament_index_cpp::get_package_share_directory("modelec_strat") +
             "/data/deposite_zone.xml";
         if (!LoadDepositeZoneFromXML(deposite_zone_path))
@@ -531,17 +533,9 @@ namespace Modelec
         pathfinding_->OnEnemyPosition(msg);
         last_enemy_pos_ = *msg;
 
-        waypoints_.clear();
-
-        WaypointMsg emptyWaypoint;
-        emptyWaypoint.x = current_pos_->x;
-        emptyWaypoint.y = current_pos_->y;
-        emptyWaypoint.theta = current_pos_->theta;
-        emptyWaypoint.id = 0;
-        emptyWaypoint.is_end = true;
-
-        waypoints_.emplace_back(emptyWaypoint);
-        SendWaypoint();
+        std_msgs::msg::Bool start_odo_msg;
+        start_odo_msg.data = false;
+        start_odo_pub_->publish(start_odo_msg);
     }
 
     void NavigationHelper::OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp
index ae19eac..2e976a5 100644
--- a/src/modelec_strat/src/strat_fms.cpp
+++ b/src/modelec_strat/src/strat_fms.cpp
@@ -48,23 +48,7 @@ namespace Modelec
                 setup_ = true;
             });
 
-        client_start_ = create_client<modelec_interfaces::srv::OdometryStart>("/odometry/start");
-        while (!client_start_->wait_for_service(std::chrono::seconds(1))) {
-            if (!rclcpp::ok()) {
-                RCLCPP_ERROR(get_logger(), "Interrupted while waiting for the service. Exiting.");
-                return;
-            }
-            RCLCPP_INFO(get_logger(), "Service not available, waiting again...");
-        }
-
-        auto request = std::make_shared<modelec_interfaces::srv::OdometryStart::Request>();
-        request->start = true;
-        client_start_->async_send_request(request, [this](rclcpp::Client<modelec_interfaces::srv::OdometryStart>::SharedFuture response) {
-            if (!response.get()->success)
-            {
-                RCLCPP_ERROR(get_logger(), "Failed to send start command.");
-            }
-        });
+        start_odo_pub_ = create_publisher<std_msgs::msg::Bool>("/odometry/start", 10);
 
         std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml";
         if (!Config::load(config_path))
@@ -134,6 +118,10 @@ namespace Modelec
         case State::INIT:
             if (setup_ && team_selected_)
             {
+                std_msgs::msg::Bool start_odo_msg;
+                start_odo_msg.data = true;
+                start_odo_pub_->publish(start_odo_msg);
+
                 Transition(State::WAIT_START, "System ready");
             }
             break;
diff --git a/src/modelec_utils/src/point.cpp b/src/modelec_utils/src/point.cpp
index 6f8a84e..7e43d7f 100644
--- a/src/modelec_utils/src/point.cpp
+++ b/src/modelec_utils/src/point.cpp
@@ -29,6 +29,6 @@ namespace Modelec
 
     Point Point::GetTakeClosePosition() const
     {
-        return GetTakePosition(130, theta);
+        return GetTakePosition(150, theta);
     }
 }