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