diff --git a/interfaces/operation_interface/msg/RobotState.msg b/interfaces/operation_interface/msg/RobotState.msg index 55734db3..22fed736 100644 --- a/interfaces/operation_interface/msg/RobotState.msg +++ b/interfaces/operation_interface/msg/RobotState.msg @@ -1,19 +1,10 @@ uint8 robot_id uint8 robot_level -uint16 remain_hp -uint16 max_hp -uint16 shooter_id1_17mm_cooling_rate -uint16 shooter_id1_17mm_cooling_limit -uint16 shooter_id1_17mm_speed_limit - -uint16 shooter_id2_17mm_cooling_rate -uint16 shooter_id2_17mm_cooling_limit -uint16 shooter_id2_17mm_speed_limit -uint16 shooter_id1_42mm_cooling_rate -uint16 shooter_id1_42mm_cooling_limit -uint16 shooter_id1_42mm_speed_limit - +uint16 current_hp +uint16 maximum_hp +uint16 shooter_barrel_cooling_value +uint16 shooter_barrel_heat_limit uint16 chassis_power_limit -bool mains_power_gimbal_output -bool mains_power_chassis_output -bool mains_power_shooter_output \ No newline at end of file +bool power_management_gimbal_output +bool power_management_chassis_output +bool power_management_shooter_output \ No newline at end of file diff --git a/perception/referee_serial/include/referee_serial/robot_state.hpp b/perception/referee_serial/include/referee_serial/robot_state.hpp index 57c8f55a..690f383f 100644 --- a/perception/referee_serial/include/referee_serial/robot_state.hpp +++ b/perception/referee_serial/include/referee_serial/robot_state.hpp @@ -27,25 +27,16 @@ class RobotState struct [[gnu::packed]] Data { - uint8_t robot_id; - uint8_t robot_level; - uint16_t remain_HP; - uint16_t max_HP; - uint16_t shooter_id1_17mm_cooling_rate; - uint16_t shooter_id1_17mm_cooling_limit; - uint16_t shooter_id1_17mm_speed_limit; - - uint16_t shooter_id2_17mm_cooling_rate; - uint16_t shooter_id2_17mm_cooling_limit; - uint16_t shooter_id2_17mm_speed_limit; - uint16_t shooter_id1_42mm_cooling_rate; - uint16_t shooter_id1_42mm_cooling_limit; - uint16_t shooter_id1_42mm_speed_limit; - - uint16_t chassis_power_limit; - uint8_t mains_power_gimbal_output : 1; - uint8_t mains_power_chassis_output : 1; - uint8_t mains_power_shooter_output : 1; + uint8_t robot_id; + uint8_t robot_level; + uint16_t current_HP; + uint16_t maximum_HP; + uint16_t shooter_barrel_cooling_value; + uint16_t shooter_barrel_heat_limit; + uint16_t chassis_power_limit; + uint8_t power_management_gimbal_output : 1; + uint8_t power_management_chassis_output : 1; + uint8_t power_management_shooter_output : 1; }; Header header; diff --git a/perception/referee_serial/src/referee_serial.cpp b/perception/referee_serial/src/referee_serial.cpp index 13768c04..71f00d47 100644 --- a/perception/referee_serial/src/referee_serial.cpp +++ b/perception/referee_serial/src/referee_serial.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include "referee_serial/crc.h" #define DEBUG false @@ -74,14 +75,19 @@ rclcpp::node_interfaces::NodeBaseInterface::SharedPtr RefereeSerial::get_node_ba void RefereeSerial::receive() { - std::vector prefix(7); // header + cmd_id + std::vector prefix; // header + cmd_id + std::vector receive_bit(1); RCLCPP_INFO(node_->get_logger(), "Receiving serial frames"); while (rclcpp::ok()) { try { - serial_driver_->port()->receive(prefix); - + serial_driver_->port()->receive(receive_bit); + prefix.push_back(receive_bit[0]); + if (prefix.size() < 7) + continue; + while (prefix.size() > 7) + prefix.erase(prefix.begin()); if (KeyMouse::is_wanted_pre(prefix)) // key mouse { handle_frame( diff --git a/perception/referee_serial/src/robot_state.cpp b/perception/referee_serial/src/robot_state.cpp index 2e121b82..9617bec0 100644 --- a/perception/referee_serial/src/robot_state.cpp +++ b/perception/referee_serial/src/robot_state.cpp @@ -2,22 +2,25 @@ #include #include #include +#include bool RobotState::is_wanted_pre(const std::vector &prefix) { - if (prefix[0] != 0xA5) return false; + if (prefix[0] != 0xA5) + return false; uint16_t length = static_cast(prefix[1]) | (static_cast(prefix[2]) << 8); uint16_t cmd_id = static_cast(prefix[5]) | (static_cast(prefix[6]) << 8); - if (cmd_id != 0x0201 || length != 27) return false; + if (cmd_id != 0x0201 || length != 13) + return false; return true; } RobotState::RobotState(const std::vector &frame) { // copy the uint8_t vector to the struct - std::copy(frame.begin(), frame.end(), reinterpret_cast(&interpreted)); + std::copy(frame.begin(), frame.end(), reinterpret_cast(&interpreted)); } operation_interface::msg::RobotState RobotState::msg() @@ -25,23 +28,13 @@ operation_interface::msg::RobotState RobotState::msg() operation_interface::msg::RobotState msg; msg.robot_id = interpreted.data.robot_id; msg.robot_level = interpreted.data.robot_level; - msg.remain_hp = interpreted.data.remain_HP; - msg.max_hp = interpreted.data.max_HP; - msg.shooter_id1_17mm_cooling_rate = interpreted.data.shooter_id1_17mm_cooling_rate; - msg.shooter_id1_17mm_cooling_limit = interpreted.data.shooter_id1_17mm_cooling_limit; - msg.shooter_id1_17mm_speed_limit = interpreted.data.shooter_id1_17mm_speed_limit; - - msg.shooter_id2_17mm_cooling_rate = interpreted.data.shooter_id2_17mm_cooling_rate; - msg.shooter_id2_17mm_cooling_limit = interpreted.data.shooter_id2_17mm_cooling_limit; - msg.shooter_id2_17mm_speed_limit = interpreted.data.shooter_id2_17mm_speed_limit; - - msg.shooter_id1_42mm_cooling_rate = interpreted.data.shooter_id1_42mm_cooling_rate; - msg.shooter_id1_42mm_cooling_limit = interpreted.data.shooter_id1_42mm_cooling_limit; - msg.shooter_id1_42mm_speed_limit = interpreted.data.shooter_id1_42mm_speed_limit; - + msg.current_hp = interpreted.data.current_HP; + msg.maximum_hp = interpreted.data.maximum_HP; + msg.shooter_barrel_cooling_value = interpreted.data.shooter_barrel_cooling_value; + msg.shooter_barrel_heat_limit = interpreted.data.shooter_barrel_heat_limit; msg.chassis_power_limit = interpreted.data.chassis_power_limit; - msg.mains_power_gimbal_output = interpreted.data.mains_power_gimbal_output; - msg.mains_power_chassis_output = interpreted.data.mains_power_chassis_output; - msg.mains_power_shooter_output = interpreted.data.mains_power_shooter_output; + msg.power_management_gimbal_output = interpreted.data.power_management_gimbal_output; + msg.power_management_chassis_output = interpreted.data.power_management_chassis_output; + msg.power_management_shooter_output = interpreted.data.power_management_shooter_output; return msg; }