default action position

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_;

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()

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);
     }
 

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>

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,
         };

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);