From c6631e4bd7e3cbe04d22552a1db5f4e94540571e Mon Sep 17 00:00:00 2001
From: acki <ackimixs@protonmail.com>
Date: Wed, 28 May 2025 18:01:22 -0400
Subject: [PATCH] servo value

---
 src/modelec_com/src/pcb_action_interface.cpp |  2 +-
 src/modelec_strat/src/navigation_helper.cpp  | 25 ++++++++++++++++----
 src/modelec_strat/src/strat_fms.cpp          |  4 ++--
 3 files changed, 23 insertions(+), 8 deletions(-)

diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp
index 157e84e..c16fe42 100644
--- a/src/modelec_com/src/pcb_action_interface.cpp
+++ b/src/modelec_com/src/pcb_action_interface.cpp
@@ -116,7 +116,7 @@ namespace Modelec
             [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr msg)
             {
                 SendOrder("SERVO" + std::to_string(msg->id), {
-                              "POS" + std::to_string(msg->pos), std::to_string(msg->angle)
+                              "POS" + std::to_string(msg->pos), std::to_string(static_cast<int>(msg->angle * 100))
                           });
             });
 
diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp
index 5d3435e..a79dc43 100644
--- a/src/modelec_strat/src/navigation_helper.cpp
+++ b/src/modelec_strat/src/navigation_helper.cpp
@@ -695,16 +695,31 @@ namespace Modelec
             }
         }*/
 
-        if (!waypoint_queue_.empty())
+        if (await_rotate_)
         {
-            waypoint_pub_->publish(waypoint_queue_.front().ToMsg());
-            waypoint_queue_.pop();
+            await_rotate_ = false;
 
-            waypoints_[waypoints_.size() - waypoint_queue_.size() - 1].reached = true;
+            waypoints_.clear();
+            for (auto& w : send_back_waypoints_)
+            {
+                waypoints_.emplace_back(w);
+            }
+            // SendWaypoint();
+            SendGoTo();
         }
         else
         {
-            waypoints_.back().reached = true;
+            if (!waypoint_queue_.empty())
+            {
+                waypoint_pub_->publish(waypoint_queue_.front().ToMsg());
+                waypoint_queue_.pop();
+
+                waypoints_[waypoints_.size() - waypoint_queue_.size() - 1].reached = true;
+            }
+            else
+            {
+                waypoints_.back().reached = true;
+            }
         }
     }
 
diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp
index 0dd6058..d40a47a 100644
--- a/src/modelec_strat/src/strat_fms.cpp
+++ b/src/modelec_strat/src/strat_fms.cpp
@@ -154,7 +154,7 @@ namespace Modelec
                     Transition(State::STOP, "Start prepare concert");
                 }*/
                 // TODO : check the time needed by the mission
-                else if (elapsed.seconds() < 75)
+                else if (elapsed.seconds() < 70)
                 {
                     Transition(State::DO_PREPARE_CONCERT, "Proceed to concert");
                 }
@@ -168,7 +168,7 @@ namespace Modelec
         case State::DO_PREPARE_CONCERT:
             if (!current_mission_)
             {
-                current_mission_ = std::make_unique<PrepareConcertMission>(nav_, action_executor_, (now - match_start_time_).seconds() < 70);
+                current_mission_ = std::make_unique<PrepareConcertMission>(nav_, action_executor_, (now - match_start_time_).seconds() < 65);
                 current_mission_->Start(shared_from_this());
             }
             current_mission_->Update();