diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index 9da96db..4fb1f21 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -33,6 +33,9 @@ namespace Modelec ASC_GO_UP_TO_TAKE_POT, PLACE_FIRST_PLATE, + // Take One floor + STICK_ALL, + // Place Pot ASC_FINAL, @@ -52,9 +55,9 @@ namespace Modelec void DeployBanner(); - void TakePot(); + void TakePot(bool two_floor = true); - void PlacePot(); + void PlacePot(bool two_floor = true); void ReInit(); diff --git a/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp index 9c58283..f118c57 100644 --- a/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp @@ -12,7 +12,7 @@ namespace Modelec { public: PrepareConcertMission(const std::shared_ptr& nav, - const std::shared_ptr& action_executor); + const std::shared_ptr& action_executor, bool two_floor = true); void Start(rclcpp::Node::SharedPtr node) override; void Update() override; @@ -38,6 +38,7 @@ namespace Modelec std::shared_ptr nav_; std::shared_ptr action_executor_; rclcpp::Node::SharedPtr node_; + bool two_floor_; std::shared_ptr column_; std::shared_ptr closestDepoZone_; diff --git a/src/modelec_strat/include/modelec_strat/strat_fms.hpp b/src/modelec_strat/include/modelec_strat/strat_fms.hpp index 1544e7d..4913c10 100644 --- a/src/modelec_strat/include/modelec_strat/strat_fms.hpp +++ b/src/modelec_strat/include/modelec_strat/strat_fms.hpp @@ -61,6 +61,7 @@ namespace Modelec bool setup_ = false; std::unique_ptr current_mission_; int team_id_ = 0; + bool is_banner_done_ = false; std::shared_ptr nav_; std::shared_ptr action_executor_; diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 0c805aa..3e7aa26 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -175,6 +175,26 @@ namespace Modelec break; case STICK_POT: { + modelec_interfaces::msg::ActionRelayState relay_top_msg; + relay_top_msg.state = true; // TODO : check that + relay_top_msg.id = 2; + relay_move_pub_->publish(relay_top_msg); + + modelec_interfaces::msg::ActionRelayState relay_bottom_msg; + relay_bottom_msg.state = true; // TODO : check that + relay_bottom_msg.id = 1; + relay_move_pub_->publish(relay_bottom_msg); + + modelec_interfaces::msg::ActionServoPos first_pot_msg; + first_pot_msg.id = 0; // TODO : to define + first_pot_msg.pos = 1; + servo_move_pub_->publish(first_pot_msg); + + modelec_interfaces::msg::ActionServoPos fourth_pot_msg; + fourth_pot_msg.id = 3; // TODO : to define + fourth_pot_msg.pos = 1; + servo_move_pub_->publish(fourth_pot_msg); + modelec_interfaces::msg::ActionServoPos second_pot_msg; second_pot_msg.id = 1; // TODO : to define second_pot_msg.pos = 1; @@ -185,7 +205,7 @@ namespace Modelec third_pot_msg.pos = 1; servo_move_pub_->publish(third_pot_msg); - step_running_ = 2; + step_running_ = 6; } break; @@ -209,6 +229,22 @@ namespace Modelec step_running_ = 1; } + break; + case STICK_ALL: + { + modelec_interfaces::msg::ActionServoPos second_pot_msg; + second_pot_msg.id = 1; // TODO : to define + second_pot_msg.pos = 1; + servo_move_pub_->publish(second_pot_msg); + + modelec_interfaces::msg::ActionServoPos third_pot_msg; + third_pot_msg.id = 2; // TODO : to define + third_pot_msg.pos = 1; + servo_move_pub_->publish(third_pot_msg); + + step_running_ = 2; + } + break; case ASC_FINAL: { @@ -291,35 +327,52 @@ namespace Modelec } } - void ActionExecutor::TakePot() + void ActionExecutor::TakePot(bool two_floor) { if (action_done_) { action_ = TAKE_POT; action_done_ = false; - step_.push(ASC_GO_DOWN); - step_.push(STICK_TO_STRUCT); - step_.push(ASC_GO_UP); - step_.push(RETRACT_BOTTOM_PLATE); - step_.push(ASC_GO_DOWN_TO_TAKE_POT); - step_.push(STICK_POT); - step_.push(ASC_GO_UP_TO_TAKE_POT); - step_.push(PLACE_FIRST_PLATE); + if (two_floor) + { + step_.push(ASC_GO_DOWN); + step_.push(STICK_TO_STRUCT); + step_.push(ASC_GO_UP); + step_.push(RETRACT_BOTTOM_PLATE); + step_.push(ASC_GO_DOWN_TO_TAKE_POT); + step_.push(STICK_POT); + step_.push(ASC_GO_UP_TO_TAKE_POT); + step_.push(PLACE_FIRST_PLATE); + } + else + { + step_.push(ASC_GO_DOWN); + step_.push(STICK_ALL); + } Update(); } } - void ActionExecutor::PlacePot() + void ActionExecutor::PlacePot(bool two_floor) { if (action_done_) { action_ = PLACE_POT; action_done_ = false; - step_.push(ASC_FINAL); - step_.push(FREE_ALL); - step_.push(REMOVE_ACTION_STEP); + + if (two_floor) + { + step_.push(ASC_FINAL); + step_.push(FREE_ALL); + step_.push(REMOVE_ACTION_STEP); + } + else + { + step_.push(FREE_ALL); + step_.push(REMOVE_ACTION_STEP); + } Update(); } diff --git a/src/modelec_strat/src/missions/prepare_concert_mission.cpp b/src/modelec_strat/src/missions/prepare_concert_mission.cpp index cb21fc8..0de39bf 100644 --- a/src/modelec_strat/src/missions/prepare_concert_mission.cpp +++ b/src/modelec_strat/src/missions/prepare_concert_mission.cpp @@ -5,8 +5,8 @@ namespace Modelec { PrepareConcertMission::PrepareConcertMission(const std::shared_ptr& nav, - const std::shared_ptr& action_executor) : - step_(GO_TO_COLUMN), + const std::shared_ptr& action_executor, bool two_floor) : + step_(GO_TO_COLUMN), two_floor_(two_floor), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { } @@ -79,7 +79,7 @@ namespace Modelec step_ = GO_CLOSE_TO_COLUMN; break; case GO_CLOSE_TO_COLUMN: - action_executor_->TakePot(); + action_executor_->TakePot(two_floor_); nav_->GetPathfinding()->RemoveObstacle(column_->GetId()); step_ = TAKE_COLUMN; @@ -175,7 +175,7 @@ namespace Modelec case GO_CLOSE_TO_PLATFORM: { - action_executor_->PlacePot(); + action_executor_->PlacePot(two_floor_); column_->SetX(closestDepoZonePoint_.x); column_->SetY(closestDepoZonePoint_.y); diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 25c69c7..5479816 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -156,8 +156,9 @@ namespace Modelec case State::SELECT_MISSION: { auto elapsed = now - match_start_time_; - // select mission in a good way there. - if (elapsed.seconds() < 2) + + // TODO : next thing to upgrade to have a good strat + if (!is_banner_done_) { Transition(State::DO_PROMOTION, "Start promotion"); } @@ -175,7 +176,7 @@ namespace Modelec case State::DO_PREPARE_CONCERT: if (!current_mission_) { - current_mission_ = std::make_unique(nav_, action_executor_); + current_mission_ = std::make_unique(nav_, action_executor_, (now - match_start_time_).seconds() < 70); current_mission_->Start(shared_from_this()); } current_mission_->Update(); @@ -207,11 +208,13 @@ namespace Modelec if (current_mission_->GetStatus() == MissionStatus::DONE) { current_mission_.reset(); + is_banner_done_ = true; Transition(State::SELECT_MISSION, "Promotion finished"); } else if (current_mission_->GetStatus() == MissionStatus::FAILED) { current_mission_.reset(); + is_banner_done_ = true; Transition(State::SELECT_MISSION, "Promotion failed"); } break;