diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp
index c16fe42..d8288cb 100644
--- a/src/modelec_com/src/pcb_action_interface.cpp
+++ b/src/modelec_com/src/pcb_action_interface.cpp
@@ -209,7 +209,7 @@ namespace Modelec
{2, {{0, M_PI_2}}},
{3, {{0, M_PI_2}}},
{4, {{0, 1.25}, {1, 0.45}}},
- {5, {{0, M_PI_2}}},
+ {5, {{0, 0}, {1, M_PI}}},
};
for (auto & [id, v] : servo_pos_mapper_)
@@ -222,10 +222,10 @@ namespace Modelec
servo_value_ = {
{0, 0},
- {1, 0},
+ {1, 1},
{2, 0},
{3, 0},
- {4, 0},
+ {4, 1},
{5, 0}
};
diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml
index 55fcf48..ddb1542 100644
--- a/src/modelec_strat/data/config.xml
+++ b/src/modelec_strat/data/config.xml
@@ -55,20 +55,20 @@
pcb_odo
- /dev/USB_ODO
-
+
+ /tmp/USB_ODO
115200
pcb_alim
- /dev/USB_ALIM
-
+
+ /tmp/USB_ALIM
115200
pcb_action
- /dev/USB_ACTION
-
+
+ /tmp/USB_ACTION
115200
diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp
index 052b259..240d503 100644
--- a/src/modelec_strat/include/modelec_strat/action_executor.hpp
+++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp
@@ -15,6 +15,7 @@ namespace Modelec
{
NONE,
DEPLOY_BANNER,
+ UNDEPLOY_BANNER,
TAKE_POT,
PLACE_POT,
DEPLOY_MAX_SIZE,
@@ -24,6 +25,7 @@ namespace Modelec
{
// Banner
DEPLOY_BANNER_STEP,
+ UNDEPLOY_BANNER_STEP,
// Take Pot
ASC_GO_DOWN,
@@ -56,6 +58,7 @@ namespace Modelec
void Update();
void DeployBanner();
+ void UndeployBanner();
void TakePot(bool two_floor = true);
diff --git a/src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp
index 9f7da7d..44e3178 100644
--- a/src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp
+++ b/src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp
@@ -27,6 +27,8 @@ namespace Modelec
{
GO_TO_FRONT,
DEPLOY_BANNER,
+ WAIT_5_SECONDS,
+ UNDEPLOY_BANNER,
GO_FORWARD,
DONE
} step_;
@@ -38,5 +40,7 @@ namespace Modelec
rclcpp::Publisher::SharedPtr score_pub_;
int mission_score_ = 0;
Point spawn_;
+
+ rclcpp::Time deploy_time_;
};
}
diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp
index af6e337..e99865a 100644
--- a/src/modelec_strat/src/action_executor.cpp
+++ b/src/modelec_strat/src/action_executor.cpp
@@ -113,6 +113,19 @@ namespace Modelec
}
break;
+
+ case UNDEPLOY_BANNER_STEP:
+ {
+ modelec_interfaces::msg::ActionServoPos msg;
+ msg.id = 5;
+ msg.pos = 0;
+ servo_move_pub_->publish(msg);
+
+ step_running_ = 1;
+ }
+
+ break;
+
case ASC_GO_DOWN:
{
modelec_interfaces::msg::ActionAscPos asc_msg;
@@ -311,6 +324,20 @@ namespace Modelec
}
}
+ void ActionExecutor::UndeployBanner()
+ {
+ if (action_done_)
+ {
+ action_ = UNDEPLOY_BANNER;
+ action_done_ = false;
+ step_running_ = 0;
+
+ step_.push(UNDEPLOY_BANNER_STEP);
+
+ Update();
+ }
+ }
+
void ActionExecutor::TakePot(bool two_floor)
{
if (action_done_)
diff --git a/src/modelec_strat/src/missions/banner_mission.cpp b/src/modelec_strat/src/missions/banner_mission.cpp
index 88d412c..745babe 100644
--- a/src/modelec_strat/src/missions/banner_mission.cpp
+++ b/src/modelec_strat/src/missions/banner_mission.cpp
@@ -47,6 +47,7 @@ namespace Modelec
case GO_TO_FRONT:
{
action_executor_->DeployBanner();
+ deploy_time_ = node_->now();
step_ = DEPLOY_BANNER;
}
@@ -54,6 +55,28 @@ namespace Modelec
break;
case DEPLOY_BANNER:
+ {
+ if ((node_->now() - deploy_time_).seconds() >= 5)
+ {
+ step_ = WAIT_5_SECONDS;
+ }
+ else
+ {
+ RCLCPP_INFO(node_->get_logger(), "Waiting for banner deployment to finish... %d seconds left", 5 - static_cast((node_->now() - deploy_time_).seconds()));
+ }
+ }
+
+ break;
+ case WAIT_5_SECONDS:
+ {
+ action_executor_->UndeployBanner();
+
+ step_ = UNDEPLOY_BANNER;
+ }
+
+ break;
+
+ case UNDEPLOY_BANNER:
{
nav_->GoTo(spawn_.x, (nav_->GetPathfinding()->robot_length_mm_ / 2) + 600, M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE);
diff --git a/src/modelec_utils/include/modelec_utils/utils.hpp b/src/modelec_utils/include/modelec_utils/utils.hpp
index 39699d8..67d5fbc 100644
--- a/src/modelec_utils/include/modelec_utils/utils.hpp
+++ b/src/modelec_utils/include/modelec_utils/utils.hpp
@@ -5,8 +5,6 @@
#include
namespace Modelec {
- #define PI 3.14159265358979323846
-
std::vector split(const std::string &s, char delim);
std::string join(const std::vector &v, const std::string &delim);