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 f317ecf..0e94436 100644 --- a/src/modelec_com/include/modelec_com/pcb_action_interface.hpp +++ b/src/modelec_com/include/modelec_com/pcb_action_interface.hpp @@ -23,11 +23,7 @@ namespace Modelec protected: std::map asc_value_mapper_; - std::map> 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}}} - }; + std::map> servo_pos_mapper_; int asc_state_ = 0; std::map servo_value_; diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index 15874fd..157e84e 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -205,10 +205,10 @@ namespace Modelec servo_pos_mapper_ = { {0, {{0, M_PI_2}}}, - {1, {{0, M_PI_2}}}, + {1, {{0, 0}, {1, 0.4}}}, {2, {{0, M_PI_2}}}, {3, {{0, M_PI_2}}}, - {4, {{0, M_PI_2}}}, + {4, {{0, 1.25}, {1, 0.45}}}, {5, {{0, M_PI_2}}}, }; @@ -216,7 +216,7 @@ namespace Modelec { for (auto & [key, angle] : v) { - SendOrder("SERVO" + std::to_string(id), {"POS" + std::to_string(key), std::to_string(angle)}); + SendOrder("SERVO" + std::to_string(id), {"POS" + std::to_string(key), std::to_string(static_cast(angle * 100))}); } } diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index aef58d6..af6e337 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -105,7 +105,7 @@ namespace Modelec case DEPLOY_BANNER_STEP: { modelec_interfaces::msg::ActionServoPos msg; - msg.id = 5; // TODO : to define + msg.id = 5; msg.pos = 1; servo_move_pub_->publish(msg); @@ -120,7 +120,7 @@ namespace Modelec asc_move_pub_->publish(asc_msg); modelec_interfaces::msg::ActionServoPos servo_action_bottom_msg; - servo_action_bottom_msg.id = 4; // TODO : to define + servo_action_bottom_msg.id = 4; servo_action_bottom_msg.pos = 1; servo_move_pub_->publish(servo_action_bottom_msg); @@ -131,23 +131,23 @@ namespace Modelec case STICK_TO_STRUCT: { modelec_interfaces::msg::ActionRelayState relay_top_msg; - relay_top_msg.state = true; // TODO : check that + relay_top_msg.state = true; 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.state = true; 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; + first_pot_msg.id = 0; + first_pot_msg.pos = 0; servo_move_pub_->publish(first_pot_msg); modelec_interfaces::msg::ActionServoPos fourth_pot_msg; - fourth_pot_msg.id = 1; // TODO : to define - fourth_pot_msg.pos = 1; + fourth_pot_msg.id = 1; + fourth_pot_msg.pos = 0; servo_move_pub_->publish(fourth_pot_msg); step_running_ = 4; @@ -167,7 +167,7 @@ namespace Modelec case RETRACT_BOTTOM_PLATE: { modelec_interfaces::msg::ActionServoPos servo_action_bottom_msg; - servo_action_bottom_msg.id = 2; // TODO : to define + servo_action_bottom_msg.id = 2; servo_action_bottom_msg.pos = 2; servo_move_pub_->publish(servo_action_bottom_msg); @@ -187,17 +187,12 @@ 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::ActionServoPos top_pot_msg; - top_pot_msg.id = 4; // TODO : to define - top_pot_msg.pos = 1; + top_pot_msg.id = 4; + top_pot_msg.pos = 0; servo_move_pub_->publish(top_pot_msg); - step_running_ = 2; + step_running_ = 1; } break; @@ -207,14 +202,19 @@ namespace Modelec asc_msg.pos = 3; asc_move_pub_->publish(asc_msg); - step_running_ = 1; + modelec_interfaces::msg::ActionServoPos top_pot_msg; + top_pot_msg.id = 4; + top_pot_msg.pos = 1; + servo_move_pub_->publish(top_pot_msg); + + step_running_ = 2; } break; case PLACE_FIRST_PLATE: { modelec_interfaces::msg::ActionServoPos action_bottom_msg; - action_bottom_msg.id = 2; // TODO : to define + action_bottom_msg.id = 2; action_bottom_msg.pos = 0; servo_move_pub_->publish(action_bottom_msg); @@ -225,8 +225,8 @@ namespace Modelec case STICK_ALL: { modelec_interfaces::msg::ActionServoPos top_pot_msg; - top_pot_msg.id = 4; // TODO : to define - top_pot_msg.pos = 1; + top_pot_msg.id = 4; + top_pot_msg.pos = 0; servo_move_pub_->publish(top_pot_msg); step_running_ = 1; @@ -246,27 +246,27 @@ namespace Modelec case FREE_ALL: { modelec_interfaces::msg::ActionRelayState relay_top_msg; - relay_top_msg.state = false; // TODO : check that + relay_top_msg.state = false; relay_top_msg.id = 2; relay_move_pub_->publish(relay_top_msg); modelec_interfaces::msg::ActionRelayState relay_bottom_msg; - relay_bottom_msg.state = false; // TODO : check that + relay_bottom_msg.state = false; 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 = 0; + first_pot_msg.id = 0; + first_pot_msg.pos = 1; servo_move_pub_->publish(first_pot_msg); modelec_interfaces::msg::ActionServoPos second_pot_msg; - second_pot_msg.id = 1; // TODO : to define - second_pot_msg.pos = 0; + second_pot_msg.id = 1; + second_pot_msg.pos = 1; servo_move_pub_->publish(second_pot_msg); modelec_interfaces::msg::ActionServoPos top_pot_msg; - top_pot_msg.id = 4; // TODO : to define + top_pot_msg.id = 4; top_pot_msg.pos = 0; servo_move_pub_->publish(top_pot_msg); @@ -281,7 +281,7 @@ namespace Modelec asc_move_pub_->publish(asc_msg); modelec_interfaces::msg::ActionServoPos servo_action_bottom_msg; - servo_action_bottom_msg.id = 3; // TODO : to define + servo_action_bottom_msg.id = 3; servo_action_bottom_msg.pos = 0; servo_move_pub_->publish(servo_action_bottom_msg);