optimize tx in mi motor

Meta-Team · Jul 30, 2024 · 226e5a2 · 226e5a2
1 parent 5c5a0fc
commit 226e5a2
Show file tree

Hide file tree

Showing 2 changed files with 36 additions and 33 deletions.
diff --git 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<int, umap<int, MiMotor*>> instances; // {port, {hid, instance}}
 
-    struct Frame
+    struct TxMsg
     {
         struct [[gnu::packed]] ExtId
         {
@@ -83,7 +83,7 @@ class MiMotor : public MotorDriver
         std::array<uint8_t, 8> 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
@@ -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,20 +101,20 @@ 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<uint32_t*>(&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<uint32_t*>(&msg.ext_id) | CAN_EFF_FLAG; // id
+    std::memcpy(tx_frame.data, msg.data.data(), 8); // data
     can_drivers[port]->send_frame(tx_frame);
 }
 
 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)