From 62b6dcea85d25cc22ab5cce0bf82e6bf2cd6230d Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 23 Dec 2023 00:32:18 +0900 Subject: [PATCH 1/2] =?UTF-8?q?CM4=E3=81=AE=E3=83=91=E3=82=B1=E3=83=83?= =?UTF-8?q?=E3=83=88=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_sender/robot_packet.hpp | 133 +++++++++++++++--- 1 file changed, 116 insertions(+), 17 deletions(-) diff --git a/crane_sender/include/crane_sender/robot_packet.hpp b/crane_sender/include/crane_sender/robot_packet.hpp index 1083e98ab..6dfb6387c 100644 --- a/crane_sender/include/crane_sender/robot_packet.hpp +++ b/crane_sender/include/crane_sender/robot_packet.hpp @@ -24,9 +24,23 @@ auto convertTwoByteToFloat(uint8_t byte_high, uint8_t byte_low, float range) -> return val; } -struct RobotCommandSerialized; +auto convertInt16ToTwoByte(int16_t val) -> std::pair +{ + uint8_t byte_low, byte_high; + byte_low = val & 0x00FF; + byte_high = (val & 0xFF00) >> 8; + return std::make_pair(byte_high, byte_low); +} -struct RobotCommand +auto convertTwoByteToInt16(uint8_t byte_high, uint8_t byte_low) -> int16_t +{ + int16_t val = (byte_high << 8) | byte_low; + return val; +} + +struct AICommandSerialized; + +struct AICommand { uint8_t HEADER; uint8_t CHECK; @@ -47,10 +61,10 @@ struct RobotCommand float KICK_POWER; float DRIBBLE_POWER; bool CHIP_ENABLE; - operator RobotCommandSerialized() const; + operator AICommandSerialized() const; }; -struct RobotCommandSerialized +struct AICommandSerialized { enum class Address { // HEADER, @@ -81,9 +95,9 @@ struct RobotCommandSerialized SIZE }; - operator RobotCommand() const + operator AICommand() const { - RobotCommand packet; + AICommand packet; #define FLOAT_FROM_2BYTE(name, range) \ packet.name = convertTwoByteToFloat( \ @@ -129,23 +143,23 @@ struct RobotCommandSerialized uint8_t data[static_cast(Address::SIZE)]; }; -RobotCommand::operator RobotCommandSerialized() const +AICommand::operator AICommandSerialized() const { - RobotCommandSerialized serialized; + AICommandSerialized serialized; #define FLOAT_TO_2BYTE(name, range) \ std::pair name##_two_byte = convertFloatToTwoByte(name, range); \ - serialized.data[static_cast(RobotCommandSerialized::Address::name##_HIGH)] = \ + serialized.data[static_cast(AICommandSerialized::Address::name##_HIGH)] = \ name##_two_byte.first; \ - serialized.data[static_cast(RobotCommandSerialized::Address::name##_LOW)] = \ + serialized.data[static_cast(AICommandSerialized::Address::name##_LOW)] = \ name##_two_byte.second #define FLOAT_TO_1BYTE(name, range) \ uint8_t name##_one_byte = static_cast(name / range * 255.0f); \ - serialized.data[static_cast(RobotCommandSerialized::Address::name)] = name##_one_byte + serialized.data[static_cast(AICommandSerialized::Address::name)] = name##_one_byte - // serialized.data[static_cast(RobotCommandSerialized::Address::HEADER)] = HEADER; - serialized.data[static_cast(RobotCommandSerialized::Address::CHECK)] = CHECK; + // serialized.data[static_cast(AICommandSerialized::Address::HEADER)] = HEADER; + serialized.data[static_cast(AICommandSerialized::Address::CHECK)] = CHECK; FLOAT_TO_2BYTE(VEL_LOCAL_SURGE, 7.0); FLOAT_TO_2BYTE(VEL_LOCAL_SWAY, 7.0); @@ -157,10 +171,9 @@ RobotCommand::operator RobotCommandSerialized() const FLOAT_TO_2BYTE(BALL_GLOBAL_X, 32.767); FLOAT_TO_2BYTE(BALL_GLOBAL_Y, 32.767); FLOAT_TO_2BYTE(TARGET_GLOBAL_THETA, M_PI); - serialized.data[static_cast(RobotCommandSerialized::Address::DRIBBLE_POWER)] = + serialized.data[static_cast(AICommandSerialized::Address::DRIBBLE_POWER)] = static_cast(DRIBBLE_POWER * 20); - serialized.data[static_cast(RobotCommandSerialized::Address::KICK_POWER)] = - [&]() -> uint8_t { + serialized.data[static_cast(AICommandSerialized::Address::KICK_POWER)] = [&]() -> uint8_t { if (CHIP_ENABLE) { return static_cast((std::round(20 * KICK_POWER) + 100)); } else { @@ -173,7 +186,7 @@ RobotCommand::operator RobotCommandSerialized() const local_flags |= (LOCAL_FEEDBACK_ENABLE << 2); local_flags |= (LOCAL_KEEPER_MODE_ENABLE << 4); - serialized.data[static_cast(RobotCommandSerialized::Address::LOCAL_FLAGS)] = local_flags; + serialized.data[static_cast(AICommandSerialized::Address::LOCAL_FLAGS)] = local_flags; #undef FLOAT_TO_1BYTE #undef FLOAT_TO_2BYTE @@ -181,4 +194,90 @@ RobotCommand::operator RobotCommandSerialized() const return serialized; } +struct CM4LocalVisionInfoSerialized; +struct CM4LocalVisionInfo +{ + int16_t x, y; + int16_t radius; + uint8_t fps; + operator CM4LocalVisionInfoSerialized() const; +}; + +struct CM4LocalVisionInfoSerialized +{ + enum class Address { + X_HIGH, + X_LOW, + Y_HIGH, + Y_LOW, + RADIUS_HIGH, + RADIUS_LOW, + FPS, + SIZE, + }; + operator CM4LocalVisionInfo() const + { + CM4LocalVisionInfo packet; + packet.x = convertTwoByteToInt16( + data[static_cast(Address::X_HIGH)], data[static_cast(Address::X_LOW)]); + packet.y = convertTwoByteToInt16( + data[static_cast(Address::Y_HIGH)], data[static_cast(Address::Y_LOW)]); + packet.radius = convertTwoByteToInt16( + data[static_cast(Address::RADIUS_HIGH)], data[static_cast(Address::RADIUS_LOW)]); + packet.fps = data[static_cast(Address::FPS)]; + return packet; + } + + uint8_t data[static_cast(Address::SIZE)]; +}; + +struct CM4CommandSerialized; +struct CM4Command +{ + CM4LocalVisionInfo local_vision_info; + AICommand ai_command; + operator CM4CommandSerialized() const; +}; + +struct CM4CommandSerialized +{ + operator CM4Command() const + { + AICommandSerialized ai_command_serialized; + for (int i = 0; i < static_cast(AICommandSerialized::Address::SIZE); i++) { + ai_command_serialized.data[i] = data[i]; + } + CM4LocalVisionInfoSerialized local_vision_info_serialized; + for (int i = 0; i < static_cast(CM4LocalVisionInfoSerialized::Address::SIZE); i++) { + local_vision_info_serialized.data[i] = + data[i + static_cast(AICommandSerialized::Address::SIZE)]; + } + + CM4Command packet; + packet.local_vision_info = static_cast(local_vision_info_serialized); + packet.ai_command = static_cast(ai_command_serialized); + return packet; + } + + uint8_t data + [static_cast(AICommandSerialized::Address::SIZE) + + static_cast(CM4LocalVisionInfoSerialized::Address::SIZE)]; +}; + +CM4Command::operator CM4CommandSerialized() const +{ + CM4CommandSerialized serialized; + AICommandSerialized ai_command_serialized = static_cast(ai_command); + for (int i = 0; i < static_cast(AICommandSerialized::Address::SIZE); i++) { + serialized.data[i] = ai_command_serialized.data[i]; + } + CM4LocalVisionInfoSerialized local_vision_info_serialized = + static_cast(local_vision_info); + for (int i = 0; i < static_cast(CM4LocalVisionInfoSerialized::Address::SIZE); i++) { + serialized.data[i + static_cast(AICommandSerialized::Address::SIZE)] = + local_vision_info_serialized.data[i]; + } + return serialized; +} + #endif //CRANE_LOCAL_PLANNER_ROBOT_PACKET_HPP From 4c92f54353702a775e633ed3a55e18a9990e7682 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 23 Dec 2023 00:47:38 +0900 Subject: [PATCH 2/2] =?UTF-8?q?=E7=A9=BA=E3=81=AERobotFeedback=E3=81=A8CM4?= =?UTF-8?q?FeedBack=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_sender/robot_packet.hpp | 62 +++++++++++++++++++ 1 file changed, 62 insertions(+) diff --git a/crane_sender/include/crane_sender/robot_packet.hpp b/crane_sender/include/crane_sender/robot_packet.hpp index 6dfb6387c..abd11657d 100644 --- a/crane_sender/include/crane_sender/robot_packet.hpp +++ b/crane_sender/include/crane_sender/robot_packet.hpp @@ -215,6 +215,7 @@ struct CM4LocalVisionInfoSerialized FPS, SIZE, }; + operator CM4LocalVisionInfo() const { CM4LocalVisionInfo packet; @@ -235,7 +236,9 @@ struct CM4CommandSerialized; struct CM4Command { CM4LocalVisionInfo local_vision_info; + AICommand ai_command; + operator CM4CommandSerialized() const; }; @@ -247,6 +250,7 @@ struct CM4CommandSerialized for (int i = 0; i < static_cast(AICommandSerialized::Address::SIZE); i++) { ai_command_serialized.data[i] = data[i]; } + CM4LocalVisionInfoSerialized local_vision_info_serialized; for (int i = 0; i < static_cast(CM4LocalVisionInfoSerialized::Address::SIZE); i++) { local_vision_info_serialized.data[i] = @@ -271,12 +275,70 @@ CM4Command::operator CM4CommandSerialized() const for (int i = 0; i < static_cast(AICommandSerialized::Address::SIZE); i++) { serialized.data[i] = ai_command_serialized.data[i]; } + CM4LocalVisionInfoSerialized local_vision_info_serialized = static_cast(local_vision_info); for (int i = 0; i < static_cast(CM4LocalVisionInfoSerialized::Address::SIZE); i++) { serialized.data[i + static_cast(AICommandSerialized::Address::SIZE)] = local_vision_info_serialized.data[i]; } + + return serialized; +} + +struct RobotFeedbackSerialized; +struct RobotFeedback +{ + operator RobotFeedbackSerialized() const; +}; + +struct RobotFeedbackSerialized +{ + enum class Address { + SIZE, + }; + + operator RobotFeedback() const + { + RobotFeedback packet; + return packet; + } + + uint8_t data[static_cast(Address::SIZE)]; +}; + +struct CM4FeedbackSerialized; +struct CM4Feedback +{ + RobotFeedback robot_feedback; + + operator CM4FeedbackSerialized() const; +}; + +struct CM4FeedbackSerialized +{ + operator CM4Feedback() const + { + RobotFeedbackSerialized robot_feedback_serialized; + for (int i = 0; i < static_cast(RobotFeedbackSerialized::Address::SIZE); i++) { + robot_feedback_serialized.data[i] = data[i]; + } + CM4Feedback packet; + packet.robot_feedback = static_cast(robot_feedback_serialized); + return packet; + } + + uint8_t data[static_cast(RobotFeedbackSerialized::Address::SIZE)]; +}; + +CM4Feedback::operator CM4FeedbackSerialized() const +{ + CM4FeedbackSerialized serialized; + RobotFeedbackSerialized robot_feedback_serialized = + static_cast(robot_feedback); + for (int i = 0; i < static_cast(RobotFeedbackSerialized::Address::SIZE); i++) { + serialized.data[i] = robot_feedback_serialized.data[i]; + } return serialized; }