From 226e5a29ed60330d4a975dc592bf657070b692ae Mon Sep 17 00:00:00 2001 From: Yao-Xinchen Date: Tue, 30 Jul 2024 16:07:14 +0800 Subject: [PATCH] optimize tx in mi motor --- .../include/motor_controller/mi_motor.h | 11 ++-- execution/motor_controller/src/mi_motor.cpp | 58 ++++++++++--------- 2 files changed, 36 insertions(+), 33 deletions(-) diff --git a/execution/motor_controller/include/motor_controller/mi_motor.h b/execution/motor_controller/include/motor_controller/mi_motor.h index 39ea115e..48805cd6 100644 --- a/execution/motor_controller/include/motor_controller/mi_motor.h +++ b/execution/motor_controller/include/motor_controller/mi_motor.h @@ -69,7 +69,7 @@ class MiMotor : public MotorDriver static umap> instances; // {port, {hid, instance}} - struct Frame + struct TxMsg { struct [[gnu::packed]] ExtId { @@ -83,7 +83,7 @@ class MiMotor : public MotorDriver std::array data; }; - Frame parsed_frame; // The frame to be sent. + TxMsg control_msg; // The control message to be sent. double position; // The current position of the motor. double velocity; // The current velocity of the motor. @@ -120,10 +120,11 @@ class MiMotor : public MotorDriver static void destroy_port(int port); /** - * @brief Sends the parsed frame to the motor. - * @note This first converts the parsed frame to a can_frame and then sends it. + * @brief Sends a message to the motor. + * @param msg The message to be sent. + * @note This first converts the message to a CAN frame before sending it. */ - void tx(); + void tx(TxMsg msg); /** * @brief The transmission loop for sending data to the motor. diff --git a/execution/motor_controller/src/mi_motor.cpp b/execution/motor_controller/src/mi_motor.cpp index b8b73b89..fd847ae8 100644 --- a/execution/motor_controller/src/mi_motor.cpp +++ b/execution/motor_controller/src/mi_motor.cpp @@ -69,18 +69,18 @@ void MiMotor::print_info() void MiMotor::goal_helper(float pos, float vel, float tor, float kp , float kd) { - parsed_frame.ext_id.mode = 1; - parsed_frame.ext_id.id = hid; - parsed_frame.ext_id.data = float_to_uint(tor, T_MIN, T_MAX, 16); - parsed_frame.ext_id.res = 0; - parsed_frame.data[0]=float_to_uint(pos,P_MIN,P_MAX,16) >> 8; - parsed_frame.data[1]=float_to_uint(pos,P_MIN,P_MAX,16); - parsed_frame.data[2]=float_to_uint(vel,V_MIN,V_MAX,16) >> 8; - parsed_frame.data[3]=float_to_uint(vel,V_MIN,V_MAX,16); - parsed_frame.data[4]=float_to_uint(kp,KP_MIN,KP_MAX,16) >> 8; - parsed_frame.data[5]=float_to_uint(kp,KP_MIN,KP_MAX,16); - parsed_frame.data[6]=float_to_uint(kd,KD_MIN,KD_MAX,16) >> 8; - parsed_frame.data[7]=float_to_uint(kd,KD_MIN,KD_MAX,16); + control_msg.ext_id.mode = 1; + control_msg.ext_id.id = hid; + control_msg.ext_id.data = float_to_uint(tor, T_MIN, T_MAX, 16); + control_msg.ext_id.res = 0; + control_msg.data[0]=float_to_uint(pos,P_MIN,P_MAX,16) >> 8; + control_msg.data[1]=float_to_uint(pos,P_MIN,P_MAX,16); + control_msg.data[2]=float_to_uint(vel,V_MIN,V_MAX,16) >> 8; + control_msg.data[3]=float_to_uint(vel,V_MIN,V_MAX,16); + control_msg.data[4]=float_to_uint(kp,KP_MIN,KP_MAX,16) >> 8; + control_msg.data[5]=float_to_uint(kp,KP_MIN,KP_MAX,16); + control_msg.data[6]=float_to_uint(kd,KD_MIN,KD_MAX,16) >> 8; + control_msg.data[7]=float_to_uint(kd,KD_MIN,KD_MAX,16); } void MiMotor::create_port(int port) @@ -101,12 +101,12 @@ void MiMotor::destroy_port(int port) } } -void MiMotor::tx() +void MiMotor::tx(TxMsg msg) { can_frame tx_frame; tx_frame.can_dlc = 8; - tx_frame.can_id = *reinterpret_cast(&parsed_frame.ext_id) | CAN_EFF_FLAG; // id - std::memcpy(tx_frame.data, parsed_frame.data.data(), 8); // data + tx_frame.can_id = *reinterpret_cast(&msg.ext_id) | CAN_EFF_FLAG; // id + std::memcpy(tx_frame.data, msg.data.data(), 8); // data can_drivers[port]->send_frame(tx_frame); } @@ -114,7 +114,7 @@ void MiMotor::tx_loop() { while (rclcpp::ok()) { - tx(); + tx(control_msg); rclcpp::sleep_for(std::chrono::milliseconds(TX_FREQ)); } } @@ -161,22 +161,24 @@ void MiMotor::rx_loop(int port) void MiMotor::start() { - parsed_frame.ext_id.mode = 3; - parsed_frame.ext_id.id = hid; - parsed_frame.ext_id.res = 0; - parsed_frame.ext_id.data = MASTER_ID; - parsed_frame.data.fill(0); // data doesn't matter - tx(); + TxMsg start_msg; + start_msg.ext_id.mode = 3; + start_msg.ext_id.id = hid; + start_msg.ext_id.res = 0; + start_msg.ext_id.data = MASTER_ID; + start_msg.data.fill(0); // data doesn't matter + tx(start_msg); } void MiMotor::stop() { - parsed_frame.ext_id.mode = 4; - parsed_frame.ext_id.id = hid; - parsed_frame.ext_id.res = 0; - parsed_frame.ext_id.data = MASTER_ID; - parsed_frame.data.fill(0); // data doesn't matter - tx(); + TxMsg stop_msg; + stop_msg.ext_id.mode = 4; + stop_msg.ext_id.id = hid; + stop_msg.ext_id.res = 0; + stop_msg.ext_id.data = MASTER_ID; + stop_msg.data.fill(0); // data doesn't matter + tx(stop_msg); } uint32_t MiMotor::float_to_uint(float x, float x_min, float x_max, int bits)