From 6af68588f5a7241ce9dfdefd9c66f07e4f3c3b1a Mon Sep 17 00:00:00 2001
From: acki <ackimixs@protonmail.com>
Date: Sun, 18 May 2025 00:04:17 -0400
Subject: [PATCH] default action position

---
 .../modelec_com/pcb_action_interface.hpp      |  6 +-
 src/modelec_com/src/pcb_action_interface.cpp  | 57 +++++++++++++++++++
 src/modelec_gui/src/pages/map_page.cpp        |  5 +-
 src/modelec_strat/data/config.xml             |  6 +-
 .../include/modelec_strat/action_executor.hpp |  2 +-
 src/modelec_strat/src/action_executor.cpp     |  2 +-
 6 files changed, 70 insertions(+), 8 deletions(-)

diff --git a/src/modelec_com/include/modelec_com/pcb_action_interface.hpp b/src/modelec_com/include/modelec_com/pcb_action_interface.hpp
index 253bb9a..ca8850a 100644
--- a/src/modelec_com/include/modelec_com/pcb_action_interface.hpp
+++ b/src/modelec_com/include/modelec_com/pcb_action_interface.hpp
@@ -22,7 +22,11 @@ namespace Modelec
 
     protected:
         std::map<int, int> asc_value_mapper_;
-        std::map<int, std::map<int, int>> servo_pos_mapper_;
+        std::map<int, std::map<int, int>> servo_pos_mapper_ = {
+            {0, {{0, 0}, {1, 0}, {2, 0}, {3, 0}}},
+            {1, {{0, 0}, {1, 0}, {2, 0}, {3, 0}}},
+            {2, {{0, 0}, {1, 0}, {2, 0}, {3, 0}}}
+        };
 
         int asc_state_ = 0;
         std::map<int, int> servo_value_;
diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp
index 607a703..acfb7ec 100644
--- a/src/modelec_com/src/pcb_action_interface.cpp
+++ b/src/modelec_com/src/pcb_action_interface.cpp
@@ -153,6 +153,63 @@ namespace Modelec
 
         relay_move_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionRelayState>(
             "action/move/relay/res", 10);
+
+
+        // TODO : servo : 90deg all default pos (0) and then 1, 2, 3 pos for each
+        asc_value_mapper_ = {
+            {0, 0},
+            {1, 100},
+            {2, 200},
+            {3, 300}
+        };
+        for (auto & [id, v] : asc_value_mapper_)
+        {
+            SendOrder("ASC", {std::to_string(id), std::to_string(v)});
+        }
+
+        asc_state_ = 3;
+
+        SendMove("ASC", {std::to_string(asc_state_)});
+
+        servo_pos_mapper_ = {
+            {0, {{0, M_PI_2}}},
+            {1, {{0, M_PI_2}}},
+            {2, {{0, M_PI_2}}},
+            {3, {{0, M_PI_2}}},
+            {4, {{0, M_PI_2}, {1, -M_PI_2}, {2, PI}}},
+        };
+
+        for (auto & [id, v] : servo_pos_mapper_)
+        {
+            for (auto & [key, angle] : v)
+            {
+                SendOrder("SERVO" + std::to_string(id), {"POS" + std::to_string(key), std::to_string(angle)});
+            }
+        }
+
+        servo_value_ = {
+            {0, 0},
+            {1, 0},
+            {2, 0},
+            {3, 0},
+            {4, 0}
+        };
+
+        for (auto & [id, v] : servo_value_)
+        {
+            SendMove("SERVO" + std::to_string(id), {"POS" + std::to_string(v)});
+        }
+
+        relay_value_ = {
+            {0, false},
+            {1, false},
+            {2, false},
+        };
+
+        for (auto & [id, v] : relay_value_)
+        {
+            SendMove("RELAY" + std::to_string(id), {std::to_string(v)});
+        }
     }
 
     PCBActionInterface::~PCBActionInterface()
diff --git a/src/modelec_gui/src/pages/map_page.cpp b/src/modelec_gui/src/pages/map_page.cpp
index 2c7c057..bb4d4d0 100644
--- a/src/modelec_gui/src/pages/map_page.cpp
+++ b/src/modelec_gui/src/pages/map_page.cpp
@@ -269,7 +269,7 @@ namespace ModelecGUI
             // -- Draw enemy position --
             if (hasEnemy)
             {
-                painter.setBrush(Qt::red);
+                painter.setBrush(Qt::darkBlue);
                 painter.drawRect((enemy_pos_.x - (enemy_width_ / 2)) * ratioBetweenMapAndWidgetX_,
                                  height() - (enemy_pos_.y + (enemy_length_ / 2)) * ratioBetweenMapAndWidgetY_,
                                  enemy_width_ * ratioBetweenMapAndWidgetX_, enemy_length_ * ratioBetweenMapAndWidgetY_);
@@ -282,7 +282,8 @@ namespace ModelecGUI
 
         QRect rect(-(robot_width_ * ratioBetweenMapAndWidgetX_ / 2), -(robot_length_ * ratioBetweenMapAndWidgetY_ / 2),
                    robot_width_ * ratioBetweenMapAndWidgetX_, robot_length_ * ratioBetweenMapAndWidgetY_);
-        painter.setBrush(Qt::green);
+        painter.setBrush(Qt::red);
+        painter.setPen(Qt::black);
         painter.drawRect(rect);
     }
 
diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml
index 37e459d..aa908d6 100644
--- a/src/modelec_strat/data/config.xml
+++ b/src/modelec_strat/data/config.xml
@@ -43,17 +43,17 @@
         <pcb>
             <pcb_odo>
                 <name>pcb_odo</name>
-                <port>/dev/pts/20</port>
+                <port>/dev/pts/8</port>
                 <baud_rate>115200</baud_rate>
             </pcb_odo>
             <pcb_alim>
                 <name>pcb_alim</name>
-                <port>/dev/pts/22</port>
+                <port>/dev/pts/12</port>
                 <baud_rate>115200</baud_rate>
             </pcb_alim>
             <pcb_action>
                 <name>pcb_action</name>
-                <port>/dev/pts/24</port>
+                <port>/dev/pts/15</port>
                 <baud_rate>115200</baud_rate>
             </pcb_action>
         </pcb>
diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp
index e40938c..1338b9c 100644
--- a/src/modelec_strat/include/modelec_strat/action_executor.hpp
+++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp
@@ -32,10 +32,10 @@ namespace Modelec
             STICK_POT,
             ASC_GO_UP_TO_TAKE_POT,
             PLACE_FIRST_PLATE,
-            ASC_FINAL,
 
 
             // Place Pot
+            ASC_FINAL,
             FREE_ALL,
             REMOVE_ACTION_STEP,
         };
diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp
index 0166e57..4cece68 100644
--- a/src/modelec_strat/src/action_executor.cpp
+++ b/src/modelec_strat/src/action_executor.cpp
@@ -306,7 +306,6 @@ namespace Modelec
             step_.push(STICK_POT);
             step_.push(ASC_GO_UP_TO_TAKE_POT);
             step_.push(PLACE_FIRST_PLATE);
-            step_.push(ASC_FINAL);
 
             Update();
         }
@@ -318,6 +317,7 @@ namespace Modelec
         {
             action_ = PLACE_POT;
             action_done_ = false;
+            step_.push(ASC_FINAL);
             step_.push(FREE_ALL);
             step_.push(REMOVE_ACTION_STEP);