robot control update

2025-09-04 10:54:06 +08:00
parent 871ab913f1
commit f180b8a57e
42 changed files with 3045 additions and 904 deletions
--- a/HiveCoreR0/.vscode/settings.json
+++ b/HiveCoreR0/.vscode/settings.json
@@ -0,0 +1,6 @@
+{
+    "files.associations": {
+        "ostream": "cpp",
+        "cmath": "cpp"
+    }
+}
--- a/HiveCoreR0/cpu26.sh
+++ b/HiveCoreR0/cpu26.sh
@@ -1,2 +1,2 @@
 source install/setup.bash
-ros2 launch ethercat_motor_drive motor_drive.launch.py
+ros2 launch robot_bringup main.launch.py
--- a/HiveCoreR0/src/ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp
+++ b/HiveCoreR0/src/ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp
@@ -61,7 +61,10 @@ protected:
  bool auto_fault_reset_ = false;
  bool auto_state_transitions_ = true;
  bool fault_reset_ = false;
+  bool enable_ = false;
+  bool last_enable_ = false;
  int fault_reset_command_interface_index_ = -1;
+  int enable_command_interface_index_ = -1;
  bool last_fault_reset_command_ = false;
  double last_position_ = std::numeric_limits<double>::quiet_NaN();

--- a/HiveCoreR0/src/ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp
+++ b/HiveCoreR0/src/ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp
@@ -44,6 +44,19 @@ void EcCiA402Drive::processData(size_t index, uint8_t * domain_address)
          fault_reset_ = true;
        }
      }
+      if (enable_command_interface_index_ >= 0) {
+        if (command_interface_ptr_->at(enable_command_interface_index_) == 0) {
+          enable_ = false;
+        }
+        if (command_interface_ptr_->at(enable_command_interface_index_) != 0 &&
+          !std::isnan(command_interface_ptr_->at(enable_command_interface_index_)))
+        {
+          enable_ = true;
+        }
+        else{
+          enable_ = false;
+        }
+      }

      if (auto_state_transitions_) {
        pdo_channels_info_[index].default_value = transition(
@@ -92,10 +105,10 @@ void EcCiA402Drive::processData(size_t index, uint8_t * domain_address)
  if (index == all_channels_.size() - 1) {  // if last entry  in domain
    if (status_word_ != last_status_word_) {
      state_ = deviceState(status_word_);
-      if (state_ != last_state_) {
-        std::cout << "STATE: " << DEVICE_STATE_STR.at(state_)
-                  << " with status word :" << status_word_ << std::endl;
-      }
+      // if (state_ != last_state_) {
+      //   std::cout << "STATE: " << DEVICE_STATE_STR.at(state_)
+      //             << " with status word :" << status_word_ << std::endl;
+      // }
    }
    initialized_ = ((state_ == STATE_OPERATION_ENABLED) &&
      (last_state_ == STATE_OPERATION_ENABLED)) ? true : false;
@@ -134,6 +147,10 @@ bool EcCiA402Drive::setupSlave(
  if (paramters_.find("command_interface/reset_fault") != paramters_.end()) {
    fault_reset_command_interface_index_ = std::stoi(paramters_["command_interface/reset_fault"]);
  }
+  
+  if (paramters_.find("command_interface/enable") != paramters_.end()) {
+    enable_command_interface_index_ = std::stoi(paramters_["command_interface/enable"]);
+  }

  return true;
 }
@@ -195,6 +212,15 @@ DeviceState EcCiA402Drive::deviceState(uint16_t status_word)
 /** returns the control word that will take device from state to next desired state */
 uint16_t EcCiA402Drive::transition(DeviceState state, uint16_t control_word)
 {
+  if (state_ == STATE_READY_TO_SWITCH_ON 
+      || state_ == STATE_SWITCH_ON 
+      || state_ == STATE_OPERATION_ENABLED) {
+    if (!enable_)
+    {
+      return (control_word & 0b01111110) | 0b00000110;
+    }
+  }
+
  switch (state) {
    case STATE_START:                     // -> STATE_NOT_READY_TO_SWITCH_ON (automatic)
      return control_word;
@@ -203,7 +229,7 @@ uint16_t EcCiA402Drive::transition(DeviceState state, uint16_t control_word)
    case STATE_SWITCH_ON_DISABLED:        // -> STATE_READY_TO_SWITCH_ON
      return (control_word & 0b01111110) | 0b00000110;
    case STATE_READY_TO_SWITCH_ON:        // -> STATE_SWITCH_ON
-      return (control_word & 0b01110111) | 0b00000111;
+      return (control_word & 0b01110111) | 0b00000111; // enable
    case STATE_SWITCH_ON:                 // -> STATE_OPERATION_ENABLED
      return (control_word & 0b01111111) | 0b00001111;
    case STATE_OPERATION_ENABLED:         // -> GOOD
--- a/HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/config/controllers.yaml
+++ b/HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/config/controllers.yaml
@@ -1,35 +1,93 @@
 controller_manager:
  ros__parameters:
-    update_rate: 1000  # Hz
+    update_rate: 500  # Hz

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

-    trajectory_controller:
-      type: joint_trajectory_controller/JointTrajectoryController
+    # trajectory_controller:
+    #   type: joint_trajectory_controller/JointTrajectoryController

-    velocity_controller:
-      type: velocity_controllers/JointGroupVelocityController
-
-    effort_controller:
-      type: effort_controllers/JointGroupEffortController
+    gpio_command_controller:
+      type: gpio_controllers/GpioCommandController


-trajectory_controller:
+# trajectory_controller:
+#   ros__parameters:
+#     command_interfaces:
+#       - position
+#     state_interfaces:
+#       - position
+#     joints:
+#       - joint_1
+
+gpio_command_controller:
  ros__parameters:
+    gpios: 
+      - joint_1
+      - joint_2
+      - joint_3
+      # - joint_4
+      # - joint_5
+      # - joint_6
+      # - joint_7
+      # - joint_8
+      # - joint_9
+      # - joint_10
+      # - joint_11
    command_interfaces:
-      - position
-    state_interfaces:
-      - position
-    joints:
-      - joint_1
-
-velocity_controller:
-  ros__parameters:
-    joints:
-      - joint_1
-
-effort_controller:
-  ros__parameters:
-    joints:
-      - joint_1
+      joint_1:
+        - interfaces:
+          - reset_fault
+          - enable
+          - position
+      joint_2:
+        - interfaces:
+          - reset_fault
+          - enable
+          - position
+      joint_3:
+        - interfaces:
+          - reset_fault
+          - enable
+          - position
+      # joint_4:
+      #   - interfaces:
+      #     # - reset_fault
+      #     - enable
+      #     - position
+      # joint_5:
+      #   - interfaces:
+      #     # - reset_fault
+      #     - enable
+      #     - position
+      # joint_6:
+      #   - interfaces:
+      #     # - reset_fault
+      #     - enable
+      #     - position
+      # joint_7:
+      #   - interfaces:
+      #     # - reset_fault
+      #     - enable
+      #     - position
+      # joint_8:
+      #   - interfaces:
+      #     - reset_fault
+      #     - enable
+      #     - position
+      # joint_9:
+      #   - interfaces:
+      #     - reset_fault
+      #     - enable
+      #     - position
+      # joint_10:
+      #   - interfaces:
+      #     - reset_fault
+      #     - enable
+      #     - position
+      # joint_11:
+      #   - interfaces:
+      #     - reset_fault
+      #     - enable
+      #     - position
--- a/HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/config/zeroErr_config
+++ b/HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/config/zeroErr_config
@@ -0,0 +1,34 @@
+# Configuration file for Maxon EPOS3 drive
+vendor_id: 0x5a65726f
+product_id: 0x00029252
+assign-activate: 0x0300   #DC synch register
+auto_fault_reset: true
+auto_enable_set: false
+
+sdo:  # sdo data to be transferred at drive startup
+  - {index: 0x1C33, sub_index: 1, type: uint16, value: 2} # Set 0x1C33:01h = 0x02  DC设置
+  # - {index: 0x6040, sub_index: 0, type: uint16, value: 0x80} # 清除错误
+  # - {index: 0x1C32, sub_index: 1, type: uint16, value: 2}
+
+rpdo: #PxPDO = receive PDO Mapping
+  - index: 0x1600
+    channels:
+      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, default: .nan, factor: 1.0, offset: 0} #target position	
+      - {index: 0x60ff, sub_index: 0, type: int32, default: 0, factor: 1.0}	#target velocity
+      - {index: 0x6071, sub_index: 0, type: int16, default: 0, factor: 1.0}	#target torque
+      - {index: 0x6072, sub_index: 0, type: uint16, default: 500}	# Max torque
+      - {index: 0x6040, sub_index: 0, type: uint16, default: 0}	# Control word
+      - {index: 0x6060, sub_index: 0, type: uint8, default: 8}	# Control operation
+      - {index: 0xf0ff, sub_index: 0, type: uint8}	# Dummy byte
+    
+tpdo:  #TxPDO = transmit PDO Mapping
+  - index: 0x1a00
+    channels:
+      - {index: 0x6064, sub_index: 0, type: int32, state_interface: position, factor: 1.0, offset: 0} #Position actual value	
+      - {index: 0x60F4, sub_index: 0, type: int32}	#Position Following error actual value
+      - {index: 0x606c, sub_index: 0, type: int32, state_interface: velocity, factor: 1.0 }	# Velocity actual value
+      - {index: 0x6077, sub_index: 0, type: int16, state_interface: effort, factor: 1.0 }	# Torque actual value
+      - {index: 0x6041, sub_index: 0, type: uint16}	# State word
+      - {index: 0x603f, sub_index: 0, type: uint16}	# Error code
+      - {index: 0x6061, sub_index: 0, type: uint8, state_interface: mode_of_operation}	        # Mode of operation display
+      - {index: 0xf0ff, sub_index: 0, type: uint8}	        # dummy byte
--- a/HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/config/zeroErr_config.yaml
+++ b/HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/config/zeroErr_config.yaml
@@ -5,14 +5,15 @@ assign-activate: 0x0300   #DC synch register
 auto_fault_reset: true
 auto_enable_set: false

-sdo:  # sdo data to be transferred at drive startup
-  - {index: 0x1C33, sub_index: 1, type: uint16, value: 2} # Set 0x1C33:01h = 0x02  DC设置
+# sdo:  # sdo data to be transferred at drive startup
+#   - {index: 0x1C33, sub_index: 1, type: uint16, value: 2} # Set 0x1C33:01h = 0x02  DC设置
+#   # - {index: 0x6040, sub_index: 0, type: uint16, value: 0x80} # 清除错误
+#   # - {index: 0x1C32, sub_index: 1, type: uint16, value: 2}

 rpdo: #PxPDO = receive PDO Mapping
  - index: 0x1600
    channels:
-      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, default: .nan, factor: 1.0, offset: 0} 
-      #target position	
+      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, default: .nan, factor: 1.0, offset: 0} #target position	
      - {index: 0x60ff, sub_index: 0, type: int32, default: 0, factor: 1.0}	#target velocity
      - {index: 0x6071, sub_index: 0, type: int16, default: 0, factor: 1.0}	#target torque
      - {index: 0x6072, sub_index: 0, type: uint16, default: 500}	# Max torque
@@ -26,7 +27,7 @@ tpdo:  #TxPDO = transmit PDO Mapping
      - {index: 0x6064, sub_index: 0, type: int32, state_interface: position, factor: 1.0, offset: 0} #Position actual value	
      - {index: 0x60F4, sub_index: 0, type: int32}	#Position Following error actual value
      - {index: 0x606c, sub_index: 0, type: int32, state_interface: velocity, factor: 1.0 }	# Velocity actual value
-      - {index: 0x6077, sub_index: 0, type: int16, factor: 1.0 }	# Torque actual value
+      - {index: 0x6077, sub_index: 0, type: int16, state_interface: effort, factor: 1.0 }	# Torque actual value
      - {index: 0x6041, sub_index: 0, type: uint16}	# State word
      - {index: 0x603f, sub_index: 0, type: uint16}	# Error code
      - {index: 0x6061, sub_index: 0, type: uint8, state_interface: mode_of_operation}	        # Mode of operation display
--- a/HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/description/ros2_control/motor_drive.ros2_control.xacro
+++ b/HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/description/ros2_control/motor_drive.ros2_control.xacro
@@ -1,30 +1,61 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">

-  <xacro:macro name="motor_drive">
+  <!-- 公共参数定义（一次修改，全局生效） -->
+  <xacro:property name="master_id" value="0" />
+  <xacro:property name="control_frequency" value="500" />
+  <xacro:property name="ec_plugin" value="ethercat_generic_plugins/EcCiA402Drive" />
+  <xacro:property name="alias" value="0" />
+  <xacro:property name="mode_of_operation" value="8" />
+  <xacro:property name="slave_config_path" value="$(find ethercat_motor_drive)/config/zeroErr_config.yaml" />

+  <!-- 子宏：封装单个关节的重复配置 -->
+  <!-- 参数：joint_name（关节名称）、ec_position（EtherCAT位置索引） -->
+  <xacro:macro name="single_joint_config" params="joint_name ec_position">
+    <joint name="${joint_name}">
+      <!-- 状态接口（位置/速度/力矩） -->
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+      
+      <!-- 命令接口（位置控制/故障重置/使能） -->
+      <command_interface name="position"/>
+      <command_interface name="reset_fault"/>
+      <command_interface name="enable"/>
+      
+      <!-- EtherCAT模块配置（复用公共参数） -->
+      <ec_module name="zeroErr">
+        <plugin>${ec_plugin}</plugin>
+        <param name="alias">${alias}</param>
+        <param name="position">${ec_position}</param>
+        <param name="mode_of_operation">${mode_of_operation}</param>
+        <param name="slave_config">${slave_config_path}</param>
+      </ec_module>
+    </joint>
+  </xacro:macro>
+
+  <!-- 主宏：电机驱动系统配置 -->
+  <xacro:macro name="motor_drive">
    <ros2_control name="motor_drive" type="system">
      <hardware>
        <plugin>ethercat_driver/EthercatDriver</plugin>
-        <param name="master_id">0</param>
-        <param name="control_frequency">1000</param>
+        <param name="master_id">${master_id}</param>
+        <param name="control_frequency">${control_frequency}</param>
      </hardware>

-      <joint name="joint_1">
-        <state_interface name="position"/>
-        <state_interface name="velocity"/>
-        <command_interface name="position"/>
-        <command_interface name="reset_fault"/>
-        <ec_module name="SV660N">
-          <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
-          <param name="alias">0</param>
-          <param name="position">0</param>
-          <param name="mode_of_operation">8</param>
-          <param name="slave_config">$(find ethercat_motor_drive)/config/zeroErr_config.yaml</param>
-        </ec_module>
-      </joint>
+      <!-- 调用子宏生成多个关节（仅需指定名称和位置索引） -->
+      <xacro:single_joint_config joint_name="joint_1" ec_position="1" />
+      <xacro:single_joint_config joint_name="joint_2" ec_position="2" />
+      <xacro:single_joint_config joint_name="joint_3" ec_position="3" />
+      <!-- <xacro:single_joint_config joint_name="joint_4" ec_position="4" />
+      <xacro:single_joint_config joint_name="joint_5" ec_position="5" />
+      <xacro:single_joint_config joint_name="joint_6" ec_position="6" /> -->
+      <!-- <xacro:single_joint_config joint_name="joint_7" ec_position="7" />
+      <xacro:single_joint_config joint_name="joint_8" ec_position="8" />
+      <xacro:single_joint_config joint_name="joint_9" ec_position="9" />
+      <xacro:single_joint_config joint_name="joint_10" ec_position="10" />
+      <xacro:single_joint_config joint_name="joint_11" ec_position="11" /> -->
    </ros2_control>
-
  </xacro:macro>

 </robot>
--- a/HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/launch/motor_drive.launch.py
+++ b/HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/launch/motor_drive.launch.py
@@ -13,9 +13,8 @@
 # limitations under the License.

 from launch import LaunchDescription
-from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration, TextSubstitution
 from launch.actions import DeclareLaunchArgument
-from launch.actions import DeclareLaunchArgument, TimerAction

 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
@@ -34,7 +33,7 @@ def generate_launch_description():
    )

    description_file = LaunchConfiguration('description_file')
-
+    
    # Get URDF via xacro
    robot_description_content = Command(
        [
@@ -62,14 +61,20 @@ def generate_launch_description():
    control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
-        parameters=[robot_description, robot_controllers],
+        parameters=[robot_description, robot_controllers,
+                        {
+                            "lock_memory": True,
+                            "thread_priority": 90
+                        }
+                    ],
        output="both",
    )
+
    robot_state_pub_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
-        output="both",
        parameters=[robot_description],
+        output='screen'
    )

    joint_state_broadcaster_spawner = Node(
@@ -78,12 +83,18 @@ def generate_launch_description():
        arguments=["joint_state_broadcaster", "-c", "/controller_manager"],
    )

-    trajectory_controller_spawner = Node(
+    gpio_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
-        arguments=["trajectory_controller", "-c", "/controller_manager"],
+        arguments=["gpio_command_controller", "-c", "/controller_manager"],
    )

+    # trajectory_controller_spawner = Node(
+    #     package="controller_manager",
+    #     executable="spawner",
+    #     arguments=["trajectory_controller", "-c", "/controller_manager"],
+    # )
+
    # velocity_controller_spawner = Node(
    #     package="controller_manager",
    #     executable="spawner",
@@ -100,12 +111,7 @@ def generate_launch_description():
        control_node,
        robot_state_pub_node,
        joint_state_broadcaster_spawner,
-        TimerAction(
-            period=30.0,  # 1 minutes delay
-            actions=[trajectory_controller_spawner],
-        ),
-        # velocity_controller_spawner,
-        # effort_controller_spawner,
+        gpio_controller_spawner,
    ]

    return LaunchDescription(
--- a/HiveCoreR0/src/joy_control/joy_control/joy_control_node.py
+++ b/HiveCoreR0/src/joy_control/joy_control/joy_control_node.py
@@ -56,7 +56,12 @@ class JoyControlNode(Node):

    def joy_callback(self, msg):
        # 处理按键
-        #TODO: 暂时不用按键功能
+        if msg.buttons[0] == 1:
+            command = 'A'
+            self.command_publisher.publish(String(data=command))
+        if msg.buttons[1] == 1:
+            command = 'B'
+            self.command_publisher.publish(String(data=command))
        
        # 处理摇杆（死区0.1）
        deadzone = 0.1
--- a/HiveCoreR0/src/mujoco_simulation/mujoco_simulation/mujoco_sim_node.py
+++ b/HiveCoreR0/src/mujoco_simulation/mujoco_simulation/mujoco_sim_node.py
@@ -1,6 +1,6 @@
 import rclpy
 from rclpy.node import Node
-from std_msgs.msg import Float64MultiArray
+from trajectory_msgs.msg import JointTrajectory
 from sensor_msgs.msg import JointState
 import mujoco
 import numpy as np
@@ -41,8 +41,8 @@ class MujocoSimNode(Node):
            self.viewer_thread.start()
        
        # ROS发布器和订阅器
-        self.joint_state_pub = self.create_publisher(JointState, 'joint_states', 10)
-        self.cmd_sub = self.create_subscription(Float64MultiArray, 'joint_commands', self.cmd_callback, 10)
+        self.joint_state_pub = self.create_publisher(JointState, '/joint_states', 10)
+        self.cmd_sub = self.create_subscription(JointTrajectory, '/trajectory_controller/joint_trajectory', self.cmd_callback, 10)
        
        self.get_logger().info(f"MuJoCo simulation node started with model: {self.full_model_path}")
        self.get_logger().info(f"Simulation rate: {sim_rate} Hz")
@@ -64,10 +64,15 @@ class MujocoSimNode(Node):

    def cmd_callback(self, msg):
        """处理接收到的关节控制指令"""
-        if len(msg.data) == self.model.nu:
-            self.joint_commands = np.array(msg.data)
+        if len(msg.points) > 0:
+            # 获取第一个轨迹点的位置指令
+            positions = msg.points[0].positions
+            if len(positions) == self.model.nu:
+                self.joint_commands = np.array(positions)
+            else:
+                self.get_logger().warn(f"Command dimension mismatch: expected {self.model.nu}, got {len(positions)}")
        else:
-            self.get_logger().warn(f"Command dimension mismatch: expected {self.model.nu}, got {len(msg.data)}")
+            self.get_logger().warn("Received empty trajectory message")

    def simulate_step(self):
        """执行单步仿真并发布状态"""
--- a/HiveCoreR0/src/robot_bringup/launch/main.launch.py
+++ b/HiveCoreR0/src/robot_bringup/launch/main.launch.py
@@ -11,6 +11,11 @@ def generate_launch_description():
        [mujoco_sim_pkg_share, "launch", "mujoco_simulation.launch.py"]
    )

+    ethercat_pkg_share = FindPackageShare(package="ethercat_motor_drive")
+    ethercat_launch_path = PathJoinSubstitution(
+        [ethercat_pkg_share, "launch", "motor_drive.launch.py"]
+    )
+
    control_pkg_share = FindPackageShare(package="robot_control")
    control_launch_path = PathJoinSubstitution(
        [control_pkg_share, "launch", "robot_control.launch.py"]
@@ -25,6 +30,10 @@ def generate_launch_description():
        PythonLaunchDescriptionSource(mujoco_sim_launch_path),
    )

+    include_ethercat = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(ethercat_launch_path),
+    )
+
    include_control = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(control_launch_path),
    )
@@ -34,8 +43,10 @@ def generate_launch_description():
    )

    return LaunchDescription([
-        include_simulation,  # 启动仿真子launch
-        
+        # include_simulation,  # 启动仿真子launch
+
+        include_ethercat,  # 启动EtherCAT子launch
+
        include_control,      # 启动控制子launch

        include_joy_control  # 启动手柄控制子launch
--- a/HiveCoreR0/src/robot_control/CMakeLists.txt
+++ b/HiveCoreR0/src/robot_control/CMakeLists.txt
@@ -11,6 +11,8 @@ find_package(rclcpp REQUIRED)
 find_package(Eigen3 REQUIRED)  # 查找Eigen3库
 find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(trajectory_msgs REQUIRED)
+find_package(control_msgs REQUIRED)

 # 关键：添加Eigen3的头文件目录
 include_directories(
@@ -18,24 +20,39 @@ include_directories(
  ${EIGEN3_INCLUDE_DIRS}  # 引入Eigen3的头文件路径
 )

-add_executable(robot_fsm 
-  src/robot_fsm.cpp
+# 定义源文件列表（便于维护）
+set(SOURCES
+  src/input_device.cpp
+  src/trapezoidal_trajectory.cpp
  src/robot_control_manager.cpp  
-  src/input_device.cpp       
+  src/robot_fsm.cpp
  src/arm_control.cpp
  src/leg_control.cpp
  src/robot_model.cpp
  src/wheel_control.cpp
+  src/main.cpp
+  src/rs485_driver.cpp
 )

-ament_target_dependencies(robot_fsm 
+# 定义头文件路径（让编译器找到自定义头文件）
+include_directories(
+  include/${PROJECT_NAME}
+)
+
+
+# 构建可执行文件
+add_executable(robot_fsm_node ${SOURCES})
+
+ament_target_dependencies(robot_fsm_node 
  rclcpp 
  geometry_msgs 
  sensor_msgs
+  trajectory_msgs
+  control_msgs
 )

 install(TARGETS
-  robot_fsm
+  robot_fsm_node
  DESTINATION lib/${PROJECT_NAME})

 install(DIRECTORY launch
--- a/HiveCoreR0/src/robot_control/include/arm_control.hpp
+++ b/HiveCoreR0/src/robot_control/include/arm_control.hpp
@@ -3,38 +3,52 @@

 #include "rclcpp/rclcpp.hpp"
 #include <Eigen/Dense>
+#include "trapezoidal_trajectory.hpp"

 namespace robot_control
 {
    class ArmControl
    {
-    public:
-        ArmControl(int total_joints);
-        ~ArmControl();
+        public:
+            ArmControl(size_t total_joints, double length1, double length2, double maxSpeed, double maxAcc,  const std::vector<double>& minLimits, const std::vector<double>& maxLimits, const std::vector<double>& home_positions);
+            ~ArmControl();

-        void MoveToZeroPoint(double duration);
+            void MoveToTargetPoint(const Eigen::Vector3d& target_pos, double duration);

-        void MoveToTargetPoint(const Eigen::Vector3d& target_pos, double duration);
+            void MoveToTargetJoint(const std::vector<double>& target_joint, double duration);

-        void MoveToTargetJoint(const std::vector<double>& target_joint, double duration);
+            bool GoHome(std::vector<double>& joint_positions, double dt);

-        void DefaultArmMovement(std::vector<double>& joint_position_desired, double duration);
+            bool Stop(std::vector<double>& joint_positions, double dt);

-    private:
+            void UpdateJointStates(const std::vector<double>& joint_positions, const std::vector<double>& joint_velocities, const std::vector<double>& joint_torques);

-        int total_joints_;         // 总关节数
+            bool IsMoving();

-        std::vector<double> joint_position_;  // 机械臂关节位置
-        std::vector<double> joint_velocity_;  // 机械臂关节速度
-        std::vector<double> joint_acceleration_;  // 机械臂关节加速度
-        std::vector<double> joint_torque_;  // 机械臂关节力矩
-        std::vector<double> joint_angle_;  // 机械臂关节角度
+        private:

-        std::vector<double> joint_position_desired_;  // 机械臂关节期望位置
+            size_t total_joints_;         // 总关节数
+            double length1_;              // 机械臂第一段长度
+            double length2_;              // 机械臂第二段长度

-        double amplitude_rad_;  // 振幅 （弧度）
-        double amplitude_deg_;  // 振幅 （角度）
-        double frequency_;        // 频率
+            double maxSpeed_;             // 最大速度
+            double maxAcc_;               // 最大加速度
+
+            TrapezoidalTrajectory* trapezoidalTrajectory_;  // 轨迹规划器
+
+            std::vector<double> joint_home_positions_;
+            std::vector<double> joint_position_;  // 机械臂关节位置
+            std::vector<double> joint_velocity_;  // 机械臂关节速度
+            std::vector<double> joint_acceleration_;  // 机械臂关节加速度
+            std::vector<double> joint_torque_;  // 机械臂关节力矩
+            
+            std::vector<double> minLimits_; 
+            std::vector<double> maxLimits_;  
+
+            std::vector<double> joint_position_desired_;  // 机械臂关节期望位置
+
+            bool is_moving_;      // 是否在运动
+            bool is_stopping_;    // 是否在停止
    };

 }
--- a/HiveCoreR0/src/robot_control/include/common_enum.hpp
+++ b/HiveCoreR0/src/robot_control/include/common_enum.hpp
@@ -0,0 +1,47 @@
+#ifndef COMMON_ENUM_HPP
+#define COMMON_ENUM_HPP
+
+namespace robot_control
+{
+    /**
+     * @brief 机器人状态枚举（全局通用）
+     */
+    enum class RobotState {
+        STOPPED,          // 停止
+        MOVING_FORWARD,   // 前进
+        MOVING_BACKWARD,  // 后退
+        TURNING_LEFT,     // 左转
+        TURNING_RIGHT,    // 右转
+        MOVING_UP,        // 上升（腰部）
+        MOVING_DOWN,      // 下降（腰部）
+        HOME_MOVING      // 回零
+    };
+
+    /**
+     * @brief 控制命令枚举（全局通用）
+     */
+    enum class ControlCommand {
+        NONE,     // 无命令
+        FORWARD,  // 前进
+        BACKWARD, // 后退
+        LEFT,     // 左转
+        RIGHT,    // 右转
+        UP,       // 上升
+        DOWN,     // 下降
+        STOP,     // 停止
+        ARM_GRAB,  // 手臂抓取
+        GO_HOME,   // 关节回零
+        ENABLE,     // 上电
+    };
+
+    /**
+     * @brief 输入设备类型枚举（全局通用）
+     */
+    enum class InputType {
+        KEYBOARD,  // 键盘
+        VOICE,     // 语音
+        JOY        // 游戏手柄
+    };
+}
+
+#endif // RSO2_ROBOT_COMMON_ENUM_HPP
--- a/HiveCoreR0/src/robot_control/include/deceleration_planner.hpp
+++ b/HiveCoreR0/src/robot_control/include/deceleration_planner.hpp
@@ -0,0 +1,109 @@
+#ifndef DECELERATION_PLANNER_H
+#define DECELERATION_PLANNER_H
+
+#include <vector>
+#include <functional>
+#include <cmath>
+#include <stdexcept>
+
+inline double calculate_decel_time(double initial_velocity, double max_decel) {
+    if (std::fabs(initial_velocity) < 1e-9) { // 速度为0，无需减速
+        return 0.0;
+    }
+    return std::fabs(initial_velocity) / max_decel; // 时间 = 速度 / 减速度
+}
+
+/**
+ * @brief 机械臂多轴同步减速插补函数
+ * @details 计算多轴从当前位置、速度以最大减速度同步减速至停止的轨迹，确保所有轴同时停止
+ * 
+ * @param current_positions 各轴当前位置 (单位: m 或 rad，与机械臂配置一致)
+ * @param current_velocities 各轴当前速度 (单位: m/s 或 rad/s)
+ * @param max_deceleration 最大减速度 (绝对值，单位: m/s² 或 rad/s²)
+ * @param time_step 插补时间步长 (单位: s，建议0.001~0.01)
+ * 
+ * @note 1. 所有轴必须具有相同数量的元素
+ *       2. 最大减速度必须为正数
+ *       3. 若某轴初始速度为0，则该轴保持静止
+ */
+void plan_deceleration_trajectory(
+    const std::vector<double>& current_positions,
+    const std::vector<double>& current_velocities,
+    double max_deceleration,
+    double time_step,
+    const std::function<void(double, const std::vector<double>&, const std::vector<double>&)>& callback
+) {
+    // 输入合法性检查
+    if (current_positions.size() != current_velocities.size()) {
+        throw std::invalid_argument("位置和速度数组长度必须相同");
+    }
+    if (max_deceleration <= 0) {
+        throw std::invalid_argument("最大减速度必须为正数");
+    }
+    if (time_step <= 0 || time_step > 1.0) {
+        throw std::invalid_argument("时间步长必须在(0, 1]范围内");
+    }
+    if (!callback) {
+        throw std::invalid_argument("回调函数不能为空");
+    }
+
+    const size_t dof = current_positions.size(); // 自由度数量
+    std::vector<double> decel_times(dof);       // 各轴理论减速时间
+    std::vector<double> actual_decel(dof);      // 各轴实际减速度（确保同步停止）
+
+    // 第一步：计算各轴理论减速时间和总减速时间（取最大值，确保所有轴同时停止）
+    double total_time = 0.0;
+    for (size_t i = 0; i < dof; ++i) {
+        decel_times[i] = calculate_decel_time(current_velocities[i], max_deceleration);
+        if (decel_times[i] > total_time) {
+            total_time = decel_times[i]; // 总时间取最长减速时间
+        }
+    }
+
+    // 特殊情况：所有轴已静止
+    if (total_time < 1e-9) {
+        callback(0.0, current_positions, current_velocities);
+        return;
+    }
+
+    // 第二步：计算各轴实际减速度（确保在总时间内减速到0）
+    for (size_t i = 0; i < dof; ++i) {
+        if (std::fabs(current_velocities[i]) < 1e-9) {
+            actual_decel[i] = 0.0; // 静止轴减速度为0
+        } else {
+            // 实际减速度 = 初始速度 / 总时间（方向与速度相反）
+            actual_decel[i] = -std::copysign(max_deceleration, current_velocities[i]) 
+                            * (decel_times[i] / total_time);
+        }
+    }
+
+    // 第三步：时间插补计算（逐步生成轨迹）
+    for (double t = 0.0; t <= total_time + 1e-9; t += time_step) {
+        // 确保最后一步精确到达总时间
+        const double current_t = std::min(t, total_time);
+        
+        std::vector<double> positions(dof);
+        std::vector<double> velocities(dof);
+
+        for (size_t i = 0; i < dof; ++i) {
+            // 计算当前速度：v(t) = v0 + a*t
+            velocities[i] = current_velocities[i] + actual_decel[i] * current_t;
+            
+            // 计算当前位置：s(t) = s0 + v0*t + 0.5*a*t²
+            positions[i] = current_positions[i] 
+                         + current_velocities[i] * current_t 
+                         + 0.5 * actual_decel[i] * current_t * current_t;
+
+            // 数值稳定性处理（避免因浮点误差出现微小负速度）
+            if (std::fabs(velocities[i]) < 1e-9) {
+                velocities[i] = 0.0;
+            }
+        }
+
+        // 回调输出当前轨迹点
+        callback(current_t, positions, velocities);
+    }
+}
+    
+
+#endif // DECELERATION_PLANNER_H
--- a/HiveCoreR0/src/robot_control/include/input_device.hpp
+++ b/HiveCoreR0/src/robot_control/include/input_device.hpp
@@ -2,76 +2,65 @@
 #define INPUT_DEVICE_H

 #include <string>
+#include "common_enum.hpp"

-// 控制命令枚举
-enum class ControlCommand {
-    NONE,
-    STOP,
-    FORWARD,
-    BACKWARD,
-    LEFT,
-    RIGHT,
-    UP,
-    DOWN
-};
+namespace robot_control 
+{
+    // 输入设备接口
+    class InputDevice 
+    {
+        protected:
+            InputType type;
+            ControlCommand command;
+            
+        public:
+            InputDevice(InputType t) : type(t) {}
+            virtual ~InputDevice() = default;
+            
+            InputType GetType() const { return type; }
+            virtual ControlCommand GetCommand() = 0;
+            virtual void SetCommand(ControlCommand command) = 0;
+            virtual std::string GetDeviceName() const = 0;
+    };

-// 输入设备类型
-enum class InputType {
-    KEYBOARD,
-    JOY,
-    VOICE
-};
+    // 键盘输入设备
+    class KeyboardInput : public InputDevice 
+    {
+        public:
+            KeyboardInput() : InputDevice(InputType::KEYBOARD) {
+                command = ControlCommand::NONE;
+            }
+            
+            ControlCommand GetCommand() override;
+            void SetCommand(ControlCommand command) override;
+            std::string GetDeviceName() const override { return "Keyboard"; }
+    };

-// 输入设备接口
-class InputDevice {
-protected:
-    InputType type;
-    ControlCommand command;
-    
-public:
-    InputDevice(InputType t) : type(t) {}
-    virtual ~InputDevice() = default;
-    
-    InputType GetType() const { return type; }
-    virtual ControlCommand GetCommand() = 0;
-    virtual void SetCommand(ControlCommand command) = 0;
-    virtual std::string GetDeviceName() const = 0;
-};
+    // 手柄输入设备
+    class JoyInput : public InputDevice 
+    {
+        public:
+            JoyInput() : InputDevice(InputType::JOY) {
+                command = ControlCommand::NONE;
+            }
+            
+            ControlCommand GetCommand() override;
+            void SetCommand(ControlCommand command) override;
+            std::string GetDeviceName() const override { return "JOY"; }
+    };

-// 键盘输入设备
-class KeyboardInput : public InputDevice {
-public:
-    KeyboardInput() : InputDevice(InputType::KEYBOARD) {
-        command = ControlCommand::NONE;
-    }
-    
-    ControlCommand GetCommand() override;
-    void SetCommand(ControlCommand command) override;
-    std::string GetDeviceName() const override { return "Keyboard"; }
-};
-
-// 手柄输入设备
-class JoyInput : public InputDevice {
-public:
-    JoyInput() : InputDevice(InputType::JOY) {
-        command = ControlCommand::NONE;
-    }
-    
-    ControlCommand GetCommand() override;
-    void SetCommand(ControlCommand command) override;
-    std::string GetDeviceName() const override { return "JOY"; }
-};
-
-// 语音输入设备
-class VoiceInput : public InputDevice {
-public:
-    VoiceInput() : InputDevice(InputType::VOICE) {
-        command = ControlCommand::NONE;
-    }
-    
-    ControlCommand GetCommand() override;
-    void SetCommand(ControlCommand command) override;
-    std::string GetDeviceName() const override { return "Voice Control"; }
-};
+    // 语音输入设备
+    class VoiceInput : public InputDevice 
+    {
+        public:
+            VoiceInput() : InputDevice(InputType::VOICE) {
+                command = ControlCommand::NONE;
+            }
+            
+            ControlCommand GetCommand() override;
+            void SetCommand(ControlCommand command) override;
+            std::string GetDeviceName() const override { return "Voice Control"; }
+    };
+}

 #endif // INPUT_DEVICE_H
--- a/HiveCoreR0/src/robot_control/include/leg_control.hpp
+++ b/HiveCoreR0/src/robot_control/include/leg_control.hpp
@@ -2,40 +2,62 @@
 #define ROBOT_CONTROL__LEGCONTROL_HPP_

 #include "rclcpp/rclcpp.hpp"
+#include "trapezoidal_trajectory.hpp"

 namespace robot_control
 {

-/**
- * @brief LegControl 类用于控制腿部运动
- * 
- * 该类提供了腿部关节的控制功能，包括关节重置、移动和计算目标位置等操作。
- */
+    /**
+     * @brief LegControl 类用于控制腿部运动
+     * 
+     * 该类提供了腿部关节的控制功能，包括关节重置、移动和计算目标位置等操作。
+     */
    class LegControl
    {
    public:
-        LegControl(int totalJointNumber, double maxSpeed, double LegLength);
+        LegControl(size_t totalJoints,double LegLength, double maxSpeed, double maxAcc, const std::vector<double>& minLimits, const std::vector<double>& maxLimits, const std::vector<double>& home_positions);
        ~LegControl();

-        void ResetJoints();
+        bool MoveToTargetJoint(std::vector<double>& joint_positions, const std::vector<double>& target_joint, double dt);

-        void Move(std::vector<double>& joint_positions, double dt_sec);
+        bool MoveUp(std::vector<double>& joint_positions, double dt);

-        void CalculateWheelTargets(std::vector<double> &joint_targets, double dt_sec);
-        
-        void setLegSpeed(double speed);
+        bool MoveDown(std::vector<double>& joint_positions, double dt);
+
+        bool Stop(std::vector<double>& joint_positions, double dt);
+
+        bool GoHome(std::vector<double>& joint_positions, double dt_sec);
+
+        void UpdateJointStates(const std::vector<double>& joint_positions, const std::vector<double>& joint_velocities, const std::vector<double>& joint_torques);
+
+        bool IsMoving();

    private:

-        int totalJointNumber_;
+        size_t total_joints_;

        double maxSpeed_;

-        double speed_;
+        double maxAcc_;

        double LegLength_;

-        std::vector<double> joint_positions_;
+        bool is_moving_;
+
+        bool is_stopping_;
+
+        TrapezoidalTrajectory* trapezoidalTrajectory_;  // 轨迹规划器
+
+        std::vector<double> joint_home_positions_;
+        std::vector<double> joint_position_;  // 机械臂关节位置
+        std::vector<double> joint_velocity_;  // 机械臂关节速度
+        std::vector<double> joint_acceleration_;  // 机械臂关节加速度
+        std::vector<double> joint_torque_;  // 机械臂关节力矩
+
+        std::vector<double> maxLimits_;  
+        std::vector<double> minLimits_;  
+
+        std::vector<double> joint_position_desired_;  // 机械臂关节期望位置

    };

--- a/HiveCoreR0/src/robot_control/include/motion_parameters.hpp
+++ b/HiveCoreR0/src/robot_control/include/motion_parameters.hpp
@@ -6,41 +6,71 @@

 namespace robot_control 
 {
+    enum class LimitType {
+        POSITION,   // 位置限制（单位：度/弧度）
+        VELOCITY,   // 速度限制（单位：度/秒/弧度/秒）
+        EFFORT      // 力矩限制（单位：N·m）
+    };
+
+    // 单个关节的限制参数：包含 max（最大值）、min（最小值）、限制类型
+    struct JointLimit {
+        int index;          // 关节索引
+        double max;          // 关节运动范围上限（如位置最大角度）
+        double min;          // 关节运动范围下限（如位置最小角度）
+        LimitType limit_type;// 限制类型（区分位置/速度/力矩）
+
+        // 构造函数：默认初始化（避免未定义值）
+        JointLimit() : index(0), max(0.0), min(0.0), limit_type(LimitType::POSITION) {}
+
+        // 带参构造函数：快速初始化
+        JointLimit(int i, double max_val, double min_val, LimitType type) 
+            :index(i), max(max_val), min(min_val), limit_type(type) {
+            if (max < min) {
+                std::cerr << "[Warning] JointLimit: max (" << max << ") < min (" << min << "), swapping values!" << std::endl;
+                std::swap(max, min);
+            }
+        }
+    };
+
    class MotionParameters 
    {
        public:
            MotionParameters()
            {
-                // 初始化参数
-                arm_amplitude_rad_ = 0.0;
-                arm_frequency_ = 0.0;
-                total_joints_ = 15;
-                phase1_notified_ = false;   
-
+                // 初始化结构参数 unit m
                wheel_radius_ = 0.09;
                wheel_separation_ = 0.26;
-                max_linear_speed_x_ = 0.2;
-                max_linear_speed_z_ = 0.2;
-                max_angular_speed_z_ = 0.2;
-
-                linear_speed_x_ = 0.1;
-                linear_speed_z_ = 0.1;
-                angular_speed_z_ = 0.1;
+                leg_length_ = 0.50;  
+                arm_length_l1_ = 0.180; 
+                arm_length_l2_ = 0.219; 
+ 
+                // 轮子速度参数
+                max_linear_speed_x_ = 10;
+                max_linear_speed_z_ = 10;
+                max_angular_speed_z_ = 50;
+                
+                // 关节速度参数
+                max_joint_velocity_ = 10;  // 10 度/秒
+                max_joint_acceleration_ = 100;  // 10 度/秒^2

                // 初始化关节索引
-                leg_joint_indices_ = {0, 1, 2, 3};  // 腿部关节索引
-                wheel_joint_indices_ = {4, 5, 6, 7}; 
-                pitch_body_indices_ = {8};  // 身体关节索引
-                arm_joint_indices_ = {9, 10, 11, 12, 13, 14};  // 手臂关节索引
-            
-                wheel_joint_directions_ = {1, -1, 1, -1};  // 轮子方向
-                leg_joint_directions_ = {-1, -1, 1, 1};  // 腿部方向
-                leg_wheel_directions_ = {1, -1, -1, 1};  // 腿部 和 腰部的耦合方向
+                wheel_joint_indices_ = {0, 1, 2, 3}; 
+                pitch_body_indices_ = {0}; 
+                left_leg_joint_indices_ = {1};  
+                right_leg_joint_indices_ = {2};
+                left_arm_joint_indices_ = {3, 4, 5, 6};  
+                right_arm_joint_indices_ = {7, 8, 9, 10};  
+
+                total_joints_ = 3 ; //left_arm_joint_indices_.size() + right_arm_joint_indices_.size() + pitch_body_indices_.size() + left_leg_joint_indices_.size() + right_leg_joint_indices_.size();
                
-
-                joint_directions_.clear();
-
-                leg_length_ = 50.0;
+                // 初始化关节方向
+                wheel_joint_directions_ = {1, -1, 1, -1};  
+                left_arm_joint_directions_ = {1, 1, 1, 1};  
+                right_arm_joint_directions_ = {1, 1, 1, 1};
+                pitch_body_directions_ = {1}; 
+                left_leg_joint_directions_ = {1};  
+                right_leg_joint_directions_ = {1};
+                leg_wheel_directions_ = {1, -1, -1, 1};  // 腿部 和 腰部的耦合方向

                // 初始化关节状态
                joint_positions_.clear();
@@ -48,47 +78,151 @@ namespace robot_control
                joint_accelerations_.clear();
                joint_efforts_.clear();

+                joint_gear_ratio_ = {100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0};  // 传动比 都一样
+                joint_resolution_ = {524288, 524288 , 524288, 524288, 524288, 524288, 524288, 524288, 524288, 524288, 524288, 524288, 524288, 524288, 524288};  // 分辨率 都一样
+
+                joint_limits_ = {
+                    JointLimit(0, 360.0, -360.0, LimitType::POSITION),
+                    JointLimit(1, 360.0, -360.0, LimitType::POSITION),
+                    JointLimit(2, 360.0, -360.0, LimitType::POSITION),
+                    JointLimit(3, 360.0, -360.0, LimitType::POSITION),
+                    JointLimit(4, 360.0, -360.0, LimitType::POSITION),
+                    JointLimit(5, 360.0, -360.0, LimitType::POSITION),
+                    JointLimit(6, 360.0, -360.0, LimitType::POSITION),
+                    JointLimit(7, 360.0, -360.0, LimitType::POSITION),
+                    JointLimit(8, 360.0, -360.0, LimitType::POSITION),
+                    JointLimit(9, 360.0, -360.0, LimitType::POSITION),
+                    JointLimit(10, 360.0, -360.0, LimitType::POSITION)
+                };
+
+                for (size_t i = 0; i < left_leg_joint_indices_.size(); i++)
+                {
+                    left_leg_max_limit_[i] = joint_limits_[left_leg_joint_indices_[i]].max;
+                    left_leg_min_limit_[i] = joint_limits_[left_leg_joint_indices_[i]].min;
+                }
+
+                for (size_t i = 0; i < right_leg_joint_indices_.size(); i++)
+                {
+                    right_leg_max_limit_[i] = joint_limits_[right_leg_joint_indices_[i]].max;
+                    right_leg_min_limit_[i] = joint_limits_[right_leg_joint_indices_[i]].min;
+                }
+
+                for (size_t i = 0; i < left_arm_joint_indices_.size(); i++)
+                {
+                    left_arm_max_limit_[i] = joint_limits_[left_arm_joint_indices_[i]].max;
+                    left_arm_min_limit_[i] = joint_limits_[left_arm_joint_indices_[i]].min;
+                }
+
+                for (size_t i = 0; i < right_arm_joint_indices_.size(); i++)
+                {
+                    right_arm_max_limit_[i] = joint_limits_[right_arm_joint_indices_[i]].max;
+                    right_arm_min_limit_[i] = joint_limits_[right_arm_joint_indices_[i]].min;
+                }
+
+
+                left_arm_home_positions_ = {
+                    0.0, 0.0, 0.0, 0.0
+                };
+                right_arm_home_positions_ = {
+                    0.0, 0.0, 0.0, 0.0
+                };
+                left_leg_home_positions_ = {
+                    0.0
+                };
+                right_leg_home_positions_ = {
+                    0.0
+                };
+                pitch_body_home_positions_ = {
+                    0.0
+                };
+
+                radian_to_degree_ = 180.0 / M_PI;
+                degree_to_radian_ = M_PI / 180.0;
+                degree_to_pulse_ = 524288.0 / 360.0;
+                pulse_to_degree_ = 360.0 / 524288.0;
+                pulse_to_radian_ = 2.0 * M_PI / 524288.0;
+                radian_to_pulse_ = 524288.0 / (2.0 * M_PI);
+
+                serial_port_ = "/dev/ttyUSB0";
+                baud_rate_ = 115200;
            };

            // 运动参数
-            double leg_target_rad_;       // 腿部目标角度（弧度）
-            double arm_amplitude_rad_;    // 手臂摆动振幅（弧度）
-            double arm_frequency_;        // 手臂摆动频率（Hz）
-            int total_joints_;            // 总关节数
-            bool phase1_notified_; // 第一阶段结束提示标记
-            double duringTime_; // 运动持续时间
-            
+
+            size_t total_joints_;           // 总关节数
+
            // 关节索引
-            std::vector<int> leg_joint_indices_;  // 腿部关节索引
-            std::vector<int> arm_joint_indices_;  // 手臂关节索引
-            std::vector<int> wheel_joint_indices_; // 轮子关节索引
-            std::vector<int> pitch_body_indices_; // 腿部轮子关节索引
-            std::vector<int> joint_directions_;   // 关节方向
+            std::vector<int> left_leg_joint_indices_;    // 腿部关节索引
+            std::vector<int> right_leg_joint_indices_;    // 腿部关节索引
+            std::vector<int> left_arm_joint_indices_;    // 手臂关节索引
+            std::vector<int> right_arm_joint_indices_;    // 手臂关节索引
+            std::vector<int> wheel_joint_indices_;  // 轮子关节索引
+            std::vector<int> pitch_body_indices_;   // 腿部轮子关节索引

            std::vector<int> wheel_joint_directions_; // 轮子关节方向
-            std::vector<int> leg_joint_directions_; // 腿部关节方向
+            std::vector<int> left_arm_joint_directions_; // 手臂关节方向
+            std::vector<int> right_arm_joint_directions_;
+            std::vector<int> pitch_body_directions_; // 身体关节方向
+            std::vector<int> left_leg_joint_directions_; // 腿部关节方向
+            std::vector<int> right_leg_joint_directions_;
            std::vector<int> leg_wheel_directions_; // 腿部 和 腰部的耦合方向
-            
+
+            std::vector<double> left_arm_home_positions_; // 左臂初始位置
+            std::vector<double> right_arm_home_positions_; // 右臂初始位置
+            std::vector<double> left_leg_home_positions_; // 左腿初始位置
+            std::vector<double> right_leg_home_positions_; // 右腿初始位置
+            std::vector<double> pitch_body_home_positions_; // 身体初始位置
+
+            // 限制参数
+            std::vector<JointLimit> joint_limits_; // 关节限制
+
+            std::vector<double> left_arm_min_limit_;
+            std::vector<double> right_arm_min_limit_;
+            std::vector<double> left_leg_min_limit_;
+            std::vector<double> right_leg_min_limit_;
+
+            std::vector<double> left_arm_max_limit_;
+            std::vector<double> right_arm_max_limit_;
+            std::vector<double> left_leg_max_limit_;
+            std::vector<double> right_leg_max_limit_;
+
            // 轮子控制相关
            double wheel_radius_;         // 轮子半径（米）
            double wheel_separation_;     // 轮距（米）
-            std::vector<double> wheel_targets_;   // 轮子目标角度（弧度）
            double max_linear_speed_x_;       // 线速度（m/s）
            double max_linear_speed_z_;       // 线速度（m/s）
            double max_angular_speed_z_;      // 角速度（rad/s）

-            double linear_speed_x_;       // 线速度（m/s）
-            double linear_speed_z_;       // 线速度（m/s）
-            double angular_speed_z_;      // 角速度（rad/s）
+            // 尺寸相关
+            double leg_length_; 
+            double arm_length_l1_;
+            double arm_length_l2_;

-            // 腿相关
-            double leg_length_; // 腿部摆动振幅（弧度）
+            double max_joint_velocity_;
+            double max_joint_acceleration_;

            // 运动状态
            std::vector<double> joint_positions_; // 关节位置（弧度）
            std::vector<double> joint_velocities_; // 关节速度（弧度/秒）
            std::vector<double> joint_accelerations_; // 关节加速度（弧度/秒^2）
            std::vector<double> joint_efforts_; // 关节力矩（牛顿米）
+
+            //关节传动比和分辨率
+            std::vector<double> joint_gear_ratio_;
+            std::vector<double> joint_resolution_;
+            
+            // 转换参数
+            double radian_to_degree_; // 弧度转角度的转换因子
+            double degree_to_radian_; // 角度转弧度的转换因子
+            double degree_to_pulse_; // 角度转脉冲的转换因子
+            double pulse_to_degree_; // 脉冲转角度的转换因子
+            double pulse_to_radian_; // 脉冲转弧度的转换因子
+            double radian_to_pulse_; // 弧度转脉冲的转换因子
+
+            // 485 参数
+            std::string serial_port_; // 串口名称
+            int baud_rate_; // 波特率
+
    };
 }

--- a/HiveCoreR0/src/robot_control/include/robot_control_manager.hpp
+++ b/HiveCoreR0/src/robot_control/include/robot_control_manager.hpp
@@ -19,44 +19,35 @@ namespace robot_control
            RobotControlManager();
            ~RobotControlManager();

-            void UpdateJointStates(sensor_msgs::msg::JointState jointStates_);
-
-            // 关节复位
-            void ResetJoints();
-
            // 控制机器人运动
-            void MoveForward(double dt);
-            void MoveBackward(double dt);
-            void TurnLeft(double dt);
-            void TurnRight(double dt);
+            void MoveForward();
+            void MoveBackward();
+            void TurnLeft();
+            void TurnRight();
            void MoveUp(double dt);
            void MoveDown(double dt);
-            void Stop();
+            void Stop(double dt);
+            bool GoHome(double dt);
+            // void ArmGrab(double dt);

            // 机器人关节指令
            std_msgs::msg::Float64MultiArray GetJointCommands();
-            void SetJointCommands(std_msgs::msg::Float64MultiArray jointCommands);
-
+            void CheckJointLimits(std_msgs::msg::Float64MultiArray& jointCommands);
+            void UpdateJointStates(const sensor_msgs::msg::JointState::SharedPtr msg);

+            MotionParameters GetMotionParameters();
+            
        private:
            void init();
-            void cleanup();

            MotionParameters motionParams_;

-            int MoveCounter_; // 移动计数器
-            double MoveTime_; // 移动时间
-
            // 控制器
-            ArmControl* armController_;
-            LegControl* legController_;
+            ArmControl* leftArmController_;
+            ArmControl* rightArmController_;
+            LegControl* leftLegController_;
+            LegControl* rightLegController_;
            WheelControl* wheelController_;
-
-            // 传感器
-            // TODO: add sensor class
-
-            // 机器人模型
-            RobotModel* robotModel_;
            
            // 运动状态
            std::vector<double> jointPositions_; // 关节位置（弧度）
@@ -64,8 +55,16 @@ namespace robot_control
            std::vector<double> jointAccelerations_; // 关节加速度（弧度/秒^2）
            std::vector<double> jointEfforts_; // 关节力矩（牛顿米）

+            bool robotInited_; // 机器人是否已经初始化
+            std::vector<bool> robotHomed_; // 关节是否已经复位
+            
+            std::vector<double> tempLeftArmCmd;
+            std::vector<double> tempRightArmCmd;
+            std::vector<double> tempLeftLegCmd;
+            std::vector<double> tempRightLegCmd;
+
            sensor_msgs::msg::JointState jointStates_;
-            std_msgs::msg::Float64MultiArray jointCommands_;
+            std_msgs::msg::Float64MultiArray jointCommands_; 

    };

--- a/HiveCoreR0/src/robot_control/include/robot_fsm.hpp
+++ b/HiveCoreR0/src/robot_control/include/robot_fsm.hpp
@@ -3,74 +3,86 @@

 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/float64_multi_array.hpp"
+#include "trajectory_msgs/msg/joint_trajectory.hpp"
 #include "std_msgs/msg/string.hpp"
 #include "input_device.hpp"
 #include "robot_control_manager.hpp"
+#include "rs485_driver.hpp"
+#include "control_msgs/msg/dynamic_interface_group_values.hpp"
+#include <filesystem>
+#include <fstream>  // 添加这行来支持文件流操作
+#include <time.h>

-// 机器人运动状态枚举
-enum class RobotState {
-    STOPPED,       // 停止
-    MOVING_FORWARD,// 前进
-    MOVING_BACKWARD,// 后退
-    TURNING_LEFT,  // 左转
-    TURNING_RIGHT, // 右转
-    MOVING_UP,     // 上升
-    MOVING_DOWN    // 下降
-};
-
-class RobotFsm : public rclcpp::Node 
+namespace robot_control
 {
-    public:
-        RobotFsm();
-        ~RobotFsm();
-        
-        // 添加输入设备
-        void AddInputDevice(InputType type, std::unique_ptr<InputDevice> device);
-        
-        // 状态机主循环
-        void ControlLoop();
-        
-        // 处理命令并转换状态
-        void ProcessCommand(ControlCommand cmd);
-        
-        // 获取当前状态
-        RobotState GetCurrentState() const { return currentState_; }
-        
-        // 状态转换为字符串
-        std::string StateToString(RobotState state) const;
+    class RobotFsm : public rclcpp::Node 
+    {
+        public:
+            RobotFsm();
+            ~RobotFsm();
+            
+            // 添加输入设备
+            void AddInputDevice(InputType type, std::unique_ptr<InputDevice> device);
+            
+            // 状态机主循环
+            void ControlLoop();
+            
+            // 处理命令并转换状态
+            void ProcessCommand(ControlCommand cmd);
+            
+            // 获取当前状态
+            RobotState GetCurrentState() const { return currentState_; }
+            
+            // 状态转换为字符串
+            std::string StateToString(RobotState state) const;

-    private:
-        rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr cmdPub_;
-        rclcpp::Subscription<std_msgs::msg::String>::SharedPtr keyboadCommandSub_;
-        rclcpp::Subscription<std_msgs::msg::String>::SharedPtr voiceCommandSub_;
-        rclcpp::Subscription<std_msgs::msg::String>::SharedPtr joyCommandSub_;
-        rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr jointStatesSub_;
+            void EnableRobot();

-        RobotState currentState_;
-        robot_control::RobotControlManager robotControlManager_;
-        std::map<InputType, std::unique_ptr<InputDevice>> inputDevices_;
+        private:
+            rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr cmdPub_;
+            rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr gpioPub_;
+            rclcpp::Subscription<std_msgs::msg::String>::SharedPtr keyboadCommandSub_;
+            rclcpp::Subscription<std_msgs::msg::String>::SharedPtr voiceCommandSub_;
+            rclcpp::Subscription<std_msgs::msg::String>::SharedPtr joyCommandSub_;
+            rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr jointStatesSub_;

-        rclcpp::TimerBase::SharedPtr controlTimer_;
-        rclcpp::Time lastTime_;  // 移至类成员
+            RobotState currentState_;
+            RobotState previousState_;
+            RobotControlManager robotControlManager_;
+            std::map<InputType, std::unique_ptr<InputDevice>> inputDevices_;

-        double executionTime_;
-        bool isRunning_;
-        bool isPaused_;
-        bool isFinished_;
+             // 文件流相关
+            std::ofstream data_file_;  // 用于写入数据的文件流
+            std::string data_file_path_;  // 数据文件路径

-        // 状态转换表
-        std::map<std::pair<RobotState, ControlCommand>, RobotState> transitionTable_;
-        
-        // 初始化状态转换规则
-        void InitTransitionTable();
-        
-        // 执行当前状态对应的动作
-        void ExecuteStateAction(double dt);
+            rclcpp::TimerBase::SharedPtr controlTimer_;
+            rclcpp::Time lastTime_;  // 移至类成员

-        void JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg);
-        void KeyboardCommandCallback(const std_msgs::msg::String::SharedPtr msg);
-        void VoiceCommandCallback(const std_msgs::msg::String::SharedPtr msg);
-        void JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg);
-};
+            bool isRunning_;
+            bool isPaused_;
+            bool isFinished_;
+            bool isRobotEnabled_;
+
+            // 状态转换表
+            std::map<std::pair<RobotState, ControlCommand>, RobotState> transitionTable_;
+            
+            // 初始化状态转换规则
+            void InitTransitionTable();
+            
+            // 执行当前状态对应的动作
+            void ExecuteStateAction(double dt);
+
+            // 执行下发轨迹
+            void Publish_joint_trajectory();
+
+            void Gpio_publish_joint_trajectory();
+
+            void JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg);
+            void KeyboardCommandCallback(const std_msgs::msg::String::SharedPtr msg);
+            void VoiceCommandCallback(const std_msgs::msg::String::SharedPtr msg);
+            void JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg);
+    };
+
+} // namespace robot_control

 #endif // ROBOT_FSM_H
--- a/HiveCoreR0/src/robot_control/include/robot_kinematics.hpp
+++ b/HiveCoreR0/src/robot_control/include/robot_kinematics.hpp
@@ -9,80 +9,80 @@
 namespace robot_control
 {

-class RobotKinematics
-{
-public:
-    /**
-     * @brief 构造函数
-     * @param robot_model 机器人模型指针
-     */
-    explicit RobotKinematics(std::shared_ptr<RobotModel> robot_model);
+    class RobotKinematics
+    {
+    public:
+        /**
+         * @brief 构造函数
+         * @param robot_model 机器人模型指针
+         */
+        explicit RobotKinematics(std::shared_ptr<RobotModel> robot_model);

-    /**
-     * @brief 析构函数
-     */
-    ~RobotKinematics() = default;
+        /**
+         * @brief 析构函数
+         */
+        ~RobotKinematics() = default;

-    // 手臂运动学
+        // 手臂运动学

-    /**
-     * @brief 计算手臂正运动学
-     * @param joint_angles 关节角度数组
-     * @return 末端执行器位置 (x, y, z)
-     */
-    Eigen::Vector3d arm_forward_kinematics(const std::vector<double>& joint_angles);
+        /**
+         * @brief 计算手臂正运动学
+         * @param joint_angles 关节角度数组
+         * @return 末端执行器位置 (x, y, z)
+         */
+        Eigen::Vector3d arm_forward_kinematics(const std::vector<double>& joint_angles);

-    /**
-     * @brief 计算手臂逆运动学
-     * @param target_pos 目标位置 (x, y, z)
-     * @param[out] joint_angles 计算得到的关节角度
-     * @return 成功返回true，失败返回false
-     */
-    bool arm_inverse_kinematics(const Eigen::Vector3d& target_pos, std::vector<double>& joint_angles);
+        /**
+         * @brief 计算手臂逆运动学
+         * @param target_pos 目标位置 (x, y, z)
+         * @param[out] joint_angles 计算得到的关节角度
+         * @return 成功返回true，失败返回false
+         */
+        bool arm_inverse_kinematics(const Eigen::Vector3d& target_pos, std::vector<double>& joint_angles);

-    // 腿部运动学
+        // 腿部运动学

-    /**
-     * @brief 计算腿部正运动学
-     * @param leg_index 腿的索引 (0-3)
-     * @param joint_angles 关节角度数组
-     * @return 足部位置 (x, y, z)
-     */
-    Eigen::Vector3d leg_forward_kinematics(size_t leg_index, const std::vector<double>& joint_angles);
+        /**
+         * @brief 计算腿部正运动学
+         * @param leg_index 腿的索引 (0-3)
+         * @param joint_angles 关节角度数组
+         * @return 足部位置 (x, y, z)
+         */
+        Eigen::Vector3d leg_forward_kinematics(size_t leg_index, const std::vector<double>& joint_angles);

-    /**
-     * @brief 计算腿部逆运动学
-     * @param leg_index 腿的索引 (0-3)
-     * @param target_pos 目标位置 (x, y, z)
-     * @param[out] joint_angles 计算得到的关节角度
-     * @return 成功返回true，失败返回false
-     */
-    bool leg_inverse_kinematics(size_t leg_index, const Eigen::Vector3d& target_pos, std::vector<double>& joint_angles);
+        /**
+         * @brief 计算腿部逆运动学
+         * @param leg_index 腿的索引 (0-3)
+         * @param target_pos 目标位置 (x, y, z)
+         * @param[out] joint_angles 计算得到的关节角度
+         * @return 成功返回true，失败返回false
+         */
+        bool leg_inverse_kinematics(size_t leg_index, const Eigen::Vector3d& target_pos, std::vector<double>& joint_angles);

-private:
-    /**
-     * @brief 创建旋转矩阵 (绕Z轴)
-     * @param angle 旋转角度(弧度)
-     * @return 旋转矩阵
-     */
-    Eigen::Matrix3d rotate_z(double angle);
+    private:
+        /**
+         * @brief 创建旋转矩阵 (绕Z轴)
+         * @param angle 旋转角度(弧度)
+         * @return 旋转矩阵
+         */
+        Eigen::Matrix3d rotate_z(double angle);

-    /**
-     * @brief 创建旋转矩阵 (绕Y轴)
-     * @param angle 旋转角度(弧度)
-     * @return 旋转矩阵
-     */
-    Eigen::Matrix3d rotate_y(double angle);
+        /**
+         * @brief 创建旋转矩阵 (绕Y轴)
+         * @param angle 旋转角度(弧度)
+         * @return 旋转矩阵
+         */
+        Eigen::Matrix3d rotate_y(double angle);

-    /**
-     * @brief 创建旋转矩阵 (绕X轴)
-     * @param angle 旋转角度(弧度)
-     * @return 旋转矩阵
-     */
-    Eigen::Matrix3d rotate_x(double angle);
+        /**
+         * @brief 创建旋转矩阵 (绕X轴)
+         * @param angle 旋转角度(弧度)
+         * @return 旋转矩阵
+         */
+        Eigen::Matrix3d rotate_x(double angle);

-    std::shared_ptr<RobotModel> robot_model_;  // 机器人模型指针
-};
+        std::shared_ptr<RobotModel> robot_model_;  // 机器人模型指针
+    };

 }  // namespace robot_model

--- a/HiveCoreR0/src/robot_control/include/robot_model.hpp
+++ b/HiveCoreR0/src/robot_control/include/robot_model.hpp
@@ -8,106 +8,106 @@
 namespace robot_control
 {

-class UrdfParser;
+    class UrdfParser;

-// 关节结构体
-struct Joint
-{
-    std::string name;          // 关节名称
-    double angle;              // 关节角度（弧度）
-    double min_angle;          // 最小角度限制
-    double max_angle;          // 最大角度限制
-    double effort_limit;       // 力/力矩限制
+    // 关节结构体
+    struct Joint
+    {
+        std::string name;          // 关节名称
+        double angle;              // 关节角度（弧度）
+        double min_angle;          // 最小角度限制
+        double max_angle;          // 最大角度限制
+        double effort_limit;       // 力/力矩限制

-    Joint(const std::string & joint_name, double min, double max, double effort)
-        : name(joint_name), angle(0.0), min_angle(min), max_angle(max), effort_limit(effort) {}
-};
+        Joint(const std::string & joint_name, double min, double max, double effort)
+            : name(joint_name), angle(0.0), min_angle(min), max_angle(max), effort_limit(effort) {}
+    };

-// 连杆结构体
-struct Link
-{
-    std::string name;          // 连杆名称
-    double length;             // 连杆长度
-    double mass;               // 连杆质量
+    // 连杆结构体
+    struct Link
+    {
+        std::string name;          // 连杆名称
+        double length;             // 连杆长度
+        double mass;               // 连杆质量

-    Link(const std::string & link_name, double len, double m)
-        : name(link_name), length(len), mass(m) {}
-};
+        Link(const std::string & link_name, double len, double m)
+            : name(link_name), length(len), mass(m) {}
+    };

-// 机械臂结构体
-struct Arm
-{
-    std::vector<std::shared_ptr<Joint>> joints;  // 关节
-    std::vector<std::shared_ptr<Link>> links;    // 连杆
-    std::string side;                            // 机械臂位置（如"left", "right"）
-};
+    // 机械臂结构体
+    struct Arm
+    {
+        std::vector<std::shared_ptr<Joint>> joints;  // 关节
+        std::vector<std::shared_ptr<Link>> links;    // 连杆
+        std::string side;                            // 机械臂位置（如"left", "right"）
+    };

-// 腿部结构体
-struct Leg
-{
-    std::vector<std::shared_ptr<Joint>> joints;  // 关节
-    std::vector<std::shared_ptr<Link>> links;    // 连杆
-    std::string side;                            // 腿部位置（如"front_left", "front_right", "rear_left", "rear_right"）
-};
+    // 腿部结构体
+    struct Leg
+    {
+        std::vector<std::shared_ptr<Joint>> joints;  // 关节
+        std::vector<std::shared_ptr<Link>> links;    // 连杆
+        std::string side;                            // 腿部位置（如"front_left", "front_right", "rear_left", "rear_right"）
+    };

-class RobotModel
-{
-    // 声明UrdfParser为友元类，使其可以访问私有成员
-    friend class UrdfParser;
-    
-public:
-    /**
-     * @brief 构造函数
-     */
-    RobotModel();
+    class RobotModel
+    {
+        // 声明UrdfParser为友元类，使其可以访问私有成员
+        friend class UrdfParser;
+        
+    public:
+        /**
+         * @brief 构造函数
+         */
+        RobotModel();

-    /**
-     * @brief 析构函数
-     */
-    ~RobotModel() = default;
+        /**
+         * @brief 析构函数
+         */
+        ~RobotModel() = default;

-    /**
-     * @brief 获取机械臂
-     * @return 机械臂对象
-     */
-    const Arm & get_arm(size_t index) const;
+        /**
+         * @brief 获取机械臂
+         * @return 机械臂对象
+         */
+        const Arm & get_arm(size_t index) const;

-    /**
-     * @brief 获取腿部
-     * @param index 腿的索引（0-3）
-     * @return 腿部对象
-     */
-    const Leg & get_leg(size_t index) const;
+        /**
+         * @brief 获取腿部
+         * @param index 腿的索引（0-3）
+         * @return 腿部对象
+         */
+        const Leg & get_leg(size_t index) const;

-    /**
-     * @brief 设置关节角度
-     * @param joint_name 关节名称
-     * @param angle 目标角度（弧度）
-     * @return 成功返回true，失败返回false
-     */
-    bool set_joint_angle(const std::string & joint_name, double angle);
+        /**
+         * @brief 设置关节角度
+         * @param joint_name 关节名称
+         * @param angle 目标角度（弧度）
+         * @return 成功返回true，失败返回false
+         */
+        bool set_joint_angle(const std::string & joint_name, double angle);

-    /**
-     * @brief 获取关节角度
-     * @param joint_name 关节名称
-     * @return 关节角度（弧度），未找到返回0.0
-     */
-    double get_joint_angle(const std::string & joint_name) const;
+        /**
+         * @brief 获取关节角度
+         * @param joint_name 关节名称
+         * @return 关节角度（弧度），未找到返回0.0
+         */
+        double get_joint_angle(const std::string & joint_name) const;

-    /**
-     * @brief 打印机器人模型信息
-     */
-    void print_model_info() const;
+        /**
+         * @brief 打印机器人模型信息
+         */
+        void print_model_info() const;

-private:
-    /**
-     * @brief 初始化机器人模型
-     */
-    void initialize_model();
+    private:
+        /**
+         * @brief 初始化机器人模型
+         */
+        void initialize_model();

-    std::vector<Arm> arms_;            // 机械臂集合 （2个机械臂）
-    std::vector<Leg> legs_;            // 腿部集合（4条腿）
-};
+        std::vector<Arm> arms_;            // 机械臂集合 （2个机械臂）
+        std::vector<Leg> legs_;            // 腿部集合（4条腿）
+    };

 }  // namespace robot_CONTROL

--- a/HiveCoreR0/src/robot_control/include/rs485_driver.hpp
+++ b/HiveCoreR0/src/robot_control/include/rs485_driver.hpp
@@ -0,0 +1,64 @@
+#ifndef RS485_DRIVER_HPP
+#define RS485_DRIVER_HPP
+
+#include <string>
+#include <vector>
+#include <termios.h>
+
+namespace robot_control
+{
+    class RS485Driver
+    {
+    public:
+        RS485Driver(size_t max_motors = 4);
+        ~RS485Driver();
+        
+        // 打开串口
+        bool openPort(const std::string &port_name, int baud_rate = 115200);
+        
+        // 关闭串口
+        void closePort();
+        
+        // 发送数据
+        bool sendData(const std::vector<uint8_t> &data);
+        
+        // 接收数据
+        bool receiveData(std::vector<uint8_t> &data, size_t max_length, int timeout_ms = 100);
+        
+        // 检查串口是否打开
+        bool isOpen() const { return is_open_; }
+
+         // 控制单个电机
+        bool controlMotor(uint8_t motor_id, int32_t speed, bool stop = false);
+        
+        // 控制所有电机
+        bool controlAllMotors(const std::vector<int32_t> &speeds);
+        
+        // 停止所有电机
+        bool stopAllMotors();
+        
+        // 计算校验和
+        uint8_t calculateChecksum(const std::vector<uint8_t> &data);
+        
+    private:
+        int serial_port_;
+        bool is_open_;
+        struct termios original_termios_;
+        size_t max_motors_;
+        
+        // 转换波特率
+        speed_t convertBaudRate(int baud_rate);
+
+        // 电机协议常量
+        static constexpr uint8_t START_BYTE = 0x64;
+        static constexpr uint8_t STOP_BYTE = 0x55;
+        static constexpr uint8_t CONTROL_CMD = 0x64;
+        static constexpr uint8_t ZERO_CMD = 0x00;
+        static constexpr uint8_t SPEED_CMD = 0x14;
+        static constexpr uint8_t DEFAULT_TIME = 0x64;
+        static constexpr uint8_t STOP_CMD = 0x02;
+        static constexpr uint8_t ALL_MOTORS_CMD = 0x03;
+    };
+}
+
+#endif // RS485_DRIVER_HPP
--- a/HiveCoreR0/src/robot_control/include/trapezoidal_trajectory.hpp
+++ b/HiveCoreR0/src/robot_control/include/trapezoidal_trajectory.hpp
@@ -0,0 +1,133 @@
+#ifndef ROBOT_CONTROL__TRAJECTORY_PLANNER_HPP_
+#define ROBOT_CONTROL__TRAJECTORY_PLANNER_HPP_
+
+#include <vector>
+#include <Eigen/Dense>
+
+namespace robot_control
+{
+
+/**
+ * @brief 梯形速度轨迹规划器类
+ * 
+ * 该类实现了梯形速度曲线的轨迹规划，支持位置、速度和加速度的计算
+ */
+class TrapezoidalTrajectory
+{
+public:
+    /**
+     * @brief 构造函数
+     * @param max_velocity 最大速度约束
+     * @param max_acceleration 最大加速度约束
+     */
+    TrapezoidalTrajectory(double max_velocity = 1.0, double max_acceleration = 0.5);
+    
+    /**
+     * @brief 析构函数
+     */
+    ~TrapezoidalTrajectory() = default;
+    
+    /**
+     * @brief 初始化轨迹规划
+     * @param start_pos 起始位置
+     * @param target_pos 目标位置
+     * @param duration 期望运动时间（若为0则自动计算最小时间）
+     */
+    void init(const std::vector<double>& start_pos, const std::vector<double>& target_pos, double duration = 0.0);
+    
+    /**
+     * @brief 初始化单关节轨迹规划
+     * @param start_pos 起始位置
+     * @param target_pos 目标位置
+     * @param duration 期望运动时间（若为0则自动计算最小时间）
+     */
+    void init(double start_pos, double target_pos, double duration = 0.0);
+    
+    /**
+     * @brief 更新轨迹，计算给定时间点的位置
+     * @param time 经过的时间
+     * @return 当前时间点的位置
+     */
+    std::vector<double> update(double time);
+    
+    /**
+     * @brief 更新单关节轨迹，计算给定时间点的位置
+     * @param time 经过的时间
+     * @return 当前时间点的位置
+     */
+    double updateSingle(double time);
+    
+    /**
+     * @brief 获取当前时间点的速度
+     * @return 当前速度
+     */
+    std::vector<double> getVelocity() const { return current_velocity_; }
+    
+    /**
+     * @brief 获取单关节当前时间点的速度
+     * @return 当前速度
+     */
+    double getSingleVelocity() const { return current_velocity_[0]; }
+    
+    /**
+     * @brief 获取当前时间点的加速度
+     * @return 当前加速度
+     */
+    std::vector<double> getAcceleration() const { return current_acceleration_; }
+    
+    /**
+     * @brief 检查轨迹是否已完成
+     * @return 若轨迹完成返回true，否则返回false
+     */
+    bool isFinished(double time) const { return time >= total_time_; }
+
+    bool stopFinished(double time) const { return time >= stop_time_; }
+    
+    /**
+     * @brief 设置最大速度
+     * @param max_velocity 新的最大速度值
+     */
+    void setMaxVelocity(double max_velocity);
+    
+    /**
+     * @brief 设置最大加速度
+     * @param max_acceleration 新的最大加速度值
+     */
+    void setMaxAcceleration(double max_acceleration);
+
+    void StopCalculate();
+
+    std::vector<double> StopUpdate(double dt);
+
+private:
+    /**
+     * @brief 计算轨迹参数
+     */
+    void calculateTrajectoryParams();
+    
+    std::vector<double> start_pos_;        // 起始位置
+    std::vector<double> target_pos_;       // 目标位置
+    std::vector<double> current_pos_;      // 当前位置
+    std::vector<double> current_velocity_; // 当前速度
+    std::vector<double> current_acceleration_; // 当前加速度
+
+    std::vector<double> actual_deceleration_;
+    
+    double max_velocity_;      // 最大速度
+    double max_acceleration_;  // 最大加速度
+    
+    double total_time_;        // 总运动时间
+    double acceleration_time_; // 加速阶段时间
+    double deceleration_time_; // 减速阶段时间
+    double cruise_time_;       // 匀速阶段时间
+    double cruise_velocity_;   // 匀速阶段速度
+
+    double stop_time_;
+    
+    bool is_single_joint_;     // 是否为单关节轨迹
+    bool is_stopping_;     // 是否正在停止
+};
+
+}  // namespace robot_control
+
+#endif  // ROBOT_CONTROL__TRAJECTORY_PLANNER_HPP_
--- a/HiveCoreR0/src/robot_control/include/urdf_parser.hpp
+++ b/HiveCoreR0/src/robot_control/include/urdf_parser.hpp
@@ -6,73 +6,73 @@
 #include <memory>
 #include <urdf/model.h>

-namespace robot_model
+namespace robot_control
 {

-class UrdfParser
-{
-public:
-    /**
-     * @brief 构造函数
-     */
-    UrdfParser() = default;
+    class UrdfParser
+    {
+    public:
+        /**
+         * @brief 构造函数
+         */
+        UrdfParser() = default;

-    /**
-     * @brief 析构函数
-     */
-    ~UrdfParser() = default;
+        /**
+         * @brief 析构函数
+         */
+        ~UrdfParser() = default;

-    /**
-     * @brief 从文件解析URDF并构建RobotModel
-     * @param urdf_file_path URDF文件路径
-     * @return 构建的RobotModel共享指针，失败返回nullptr
-     */
-    std::shared_ptr<RobotModel> parse_from_file(const std::string& urdf_file_path);
+        /**
+         * @brief 从文件解析URDF并构建RobotModel
+         * @param urdf_file_path URDF文件路径
+         * @return 构建的RobotModel共享指针，失败返回nullptr
+         */
+        std::shared_ptr<RobotModel> parse_from_file(const std::string& urdf_file_path);

-    /**
-     * @brief 从XML字符串解析URDF并构建RobotModel
-     * @param urdf_xml URDF XML字符串
-     * @return 构建的RobotModel共享指针，失败返回nullptr
-     */
-    std::shared_ptr<RobotModel> parse_from_string(const std::string& urdf_xml);
+        /**
+         * @brief 从XML字符串解析URDF并构建RobotModel
+         * @param urdf_xml URDF XML字符串
+         * @return 构建的RobotModel共享指针，失败返回nullptr
+         */
+        std::shared_ptr<RobotModel> parse_from_string(const std::string& urdf_xml);

-private:
-    /**
-     * @brief 解析URDF模型并构建RobotModel
-     * @param model URDF模型
-     * @return 构建的RobotModel共享指针
-     */
-    std::shared_ptr<RobotModel> build_robot_model(const urdf::Model& model);
+    private:
+        /**
+         * @brief 解析URDF模型并构建RobotModel
+         * @param model URDF模型
+         * @return 构建的RobotModel共享指针
+         */
+        std::shared_ptr<RobotModel> build_robot_model(const urdf::Model& model);

-    /**
-     * @brief 解析连杆信息
-     * @param link URDF连杆
-     * @return 构建的Link共享指针
-     */
-    std::shared_ptr<Link> parse_link(const urdf::LinkConstSharedPtr& link);
+        /**
+         * @brief 解析连杆信息
+         * @param link URDF连杆
+         * @return 构建的Link共享指针
+         */
+        std::shared_ptr<Link> parse_link(const urdf::LinkConstSharedPtr& link);

-    /**
-     * @brief 解析关节信息
-     * @param joint URDF关节
-     * @return 构建的Joint共享指针
-     */
-    std::shared_ptr<Joint> parse_joint(const urdf::JointConstSharedPtr& joint);
+        /**
+         * @brief 解析关节信息
+         * @param joint URDF关节
+         * @return 构建的Joint共享指针
+         */
+        std::shared_ptr<Joint> parse_joint(const urdf::JointConstSharedPtr& joint);

-    /**
-     * @brief 识别关节所属部分（手臂、腿部等）
-     * @param joint_name 关节名称
-     * @return 关节所属部分的字符串标识
-     */
-    std::string identify_joint_part(const std::string& joint_name);
+        /**
+         * @brief 识别关节所属部分（手臂、腿部等）
+         * @param joint_name 关节名称
+         * @return 关节所属部分的字符串标识
+         */
+        std::string identify_joint_part(const std::string& joint_name);

-    /**
-     * @brief 识别腿部位置（前左、前右、后左、后右）
-     * @param joint_name 关节名称
-     * @return 腿部位置字符串
-     */
-    std::string identify_leg_side(const std::string& joint_name);
-};
+        /**
+         * @brief 识别腿部位置（前左、前右、后左、后右）
+         * @param joint_name 关节名称
+         * @return 腿部位置字符串
+         */
+        std::string identify_leg_side(const std::string& joint_name);
+    };

-}  // namespace robot_model
+} 

 #endif  // ROBOT_MODEL__URDF_PARSER_HPP_
--- a/HiveCoreR0/src/robot_control/include/wheel_control.hpp
+++ b/HiveCoreR0/src/robot_control/include/wheel_control.hpp
@@ -3,38 +3,50 @@

 #include "rclcpp/rclcpp.hpp"
 #include <Eigen/Dense>
+#include "trapezoidal_trajectory.hpp"
+#include "rs485_driver.hpp"

 namespace robot_control
 {
    class WheelControl
    {
    public:
-        WheelControl(int totalJoints, double wheelRadius, double wheelSeparation, double linearSpeed, double angularSpeed);
+        WheelControl(int totalJoints, double wheelRadius, double wheelSeparation, double linearSpeed, double angularSpeed, std::vector<int32_t> wheelDirection);
        ~WheelControl();

        void ResetJoints();

-        void Move(std::vector<double>& jointPositions, double dtSec);
-
-        void CalculateWheelTargets(std::vector<double> &jointTargets, double dtSec);
+        void CalculateWheelSpeeds();
        
        void setLinearAngularSpeed(double linearSpeed, double angularSpeed);

+        void ExecuteCommand();
+
    private:

        int totalJoints_;         // 总关节数

+        RS485Driver rs485_driver_;  // 串口通信
+
+        TrapezoidalTrajectory wheel_trajectory_;  // 轮子轨迹（速度模式）
+
        // 轮子控制相关
        double wheelRadius_;         // 轮子半径（米）
        double wheelSeparation_;     // 轮距（米）

-        std::vector<double> wheelTargets_;   // 轮子目标角度（弧度）
-
-        double linearSpeed_ = 0.0;       // 线速度（m/s）
-        double angularSpeed_ = 0.0;      // 角速度（rad/s）
+        double linearSpeed_;       // 线速度（m/s）
+        double angularSpeed_;      // 角速度（rad/s）

        double maxLinearSpeed_;         // 最大线速度（m/s）
        double maxAngularSpeed_;        // 最大角速度（rad/s）
+
+        bool isSendFinished_;  // 是否发送完成
+
+        std::vector<int32_t> wheelSpeed_; // 轮子速度（rpm）
+
+        std::vector<int32_t> previousWheelSpeed_; // 轮子速度（rpm）
+
+        std::vector<int32_t> wheelDirection_; // 关节位置（rad）
    };
 }

--- a/HiveCoreR0/src/robot_control/launch/robot_control.launch.py
+++ b/HiveCoreR0/src/robot_control/launch/robot_control.launch.py
@@ -6,8 +6,8 @@ def generate_launch_description():
    # 创建节点启动描述
    robot_fsm_node = Node(
        package='robot_control',  # 功能包名称
-        executable='robot_fsm',  # 可执行文件名称
-        name='robot_fsm',  # 节点名称（可自定义）
+        executable='robot_fsm_node',  # 可执行文件名称
+        name='robot_fsm_node',  # 节点名称（可自定义）
        output='screen',  # 输出到屏幕
        parameters=[{"use_sim_time": False}]  # 启用模拟时间
    )
--- a/HiveCoreR0/src/robot_control/src/arm_control.cpp
+++ b/HiveCoreR0/src/robot_control/src/arm_control.cpp
@@ -1,84 +1,208 @@
-#include "rclcpp/rclcpp.hpp"
-#include "std_msgs/msg/float64_multi_array.hpp"
-#include <cmath>
-#include <chrono>
 #include "arm_control.hpp"
+#include <stdexcept>
+#include <vector>

-using namespace std::chrono_literals;
-using namespace robot_control;
-
-ArmControl::ArmControl(int total_joints)
+namespace robot_control
 {
-    // 初始化成员变量
-    total_joints_ = total_joints;

-    // 初始化正弦运动参数
-    amplitude_deg_ = 0.5;  // 振幅（弧度）
-    frequency_ = 0.5;      // 频率（Hz）
-
-    // 初始化成员数组
+ArmControl::ArmControl(size_t total_joints, double length1, double length2, double maxSpeed, double maxAcc,  const std::vector<double>& minLimits, const std::vector<double>& maxLimits, const std::vector<double>& home_positions)
+    : total_joints_(total_joints),
+      length1_(length1),
+      length2_(length2),
+      maxSpeed_(maxSpeed),
+      maxAcc_(maxAcc),
+      joint_home_positions_(home_positions),
+      minLimits_(minLimits),
+      maxLimits_(maxLimits)
+{
+    if (total_joints_ <= 0)
+        throw std::invalid_argument("Total joints must be positive");
+    
+    // 初始化关节状态向量
    joint_position_.resize(total_joints_, 0.0);
    joint_velocity_.resize(total_joints_, 0.0);
    joint_acceleration_.resize(total_joints_, 0.0);
    joint_torque_.resize(total_joints_, 0.0);
    joint_position_desired_.resize(total_joints_, 0.0);
+    joint_home_positions_.resize(total_joints_, 0.0);

-    // 初始化正弦运动参数
-    amplitude_rad_ = amplitude_deg_ * M_PI / 180.0;  // 将角度转换为弧度
+    // 设置零点位置
+    joint_home_positions_ = home_positions;
+    
+    // 初始化梯形轨迹规划器
+    trapezoidalTrajectory_ = new TrapezoidalTrajectory(10.0, 100);

+    is_moving_ = false;
 }

 ArmControl::~ArmControl()
 {
-    // 析构函数
+    delete trapezoidalTrajectory_;
 }

-void ArmControl::DefaultArmMovement(std::vector<double>& joint_position_desired, double duration)
-{
-    double sine_pos = amplitude_rad_ * std::sin(2 * M_PI * frequency_ * duration);
-
-    for (int i = 0; i < total_joints_; i++)
-    {
-        joint_position_desired_[i] = sine_pos;
-        joint_position_desired = joint_position_desired_;
-    }
-}
-
-void ArmControl::MoveToZeroPoint(double duration)
-{
-    std::vector<double> joint_position_desired(total_joints_, 0.0);
-    MoveToTargetJoint(joint_position_desired, duration);
-}

 void ArmControl::MoveToTargetPoint(const Eigen::Vector3d& target_pos, double duration)
 {
-    // 计算每个关节的目标位置
-    for (int i = 0; i < total_joints_; i++)
-    {
-        // TODO: Inverse kinematic calculation
-        joint_position_desired_[i] = 0.0;  // Placeholder value
-    }
-
-    // 计算每个关节的所需时间
+    // 注意：这里需要运动学逆解将笛卡尔空间目标位置转换为关节空间
+    // 为简化示例，这里假设已完成逆解，直接使用虚拟关节位置
+    std::vector<double> joint_target(total_joints_, 0.0);
+    
+    // 实际应用中应在此处添加逆运动学求解
+    // joint_target = inverse_kinematics(target_pos);
+    
+    // 使用梯形轨迹规划移动到目标关节位置
+    MoveToTargetJoint(joint_target, duration);
 }

-void ArmControl::MoveToTargetJoint(const std::vector<double>& joint_position_desired, double duration)
+void ArmControl::MoveToTargetJoint(const std::vector<double>& target_joint, double duration)
 {
-    // 计算每个关节的所需时间
-    double time_step = 0.01;  // 时间步长（秒）
-    int num_steps = duration / time_step;
+    if (target_joint.size() != total_joints_)
+        throw std::invalid_argument("Target joint vector size mismatch");
+    
+    // 初始化轨迹规划器
+    trapezoidalTrajectory_->init(joint_position_, target_joint, duration);
+    
+    static double elapsed_time = 0.0;

-    // 发布目标关节位置
-    for (int i = 0; i < num_steps; i++)
+    // 更新轨迹
+    joint_position_ = trapezoidalTrajectory_->update(elapsed_time);
+    joint_velocity_ = trapezoidalTrajectory_->getVelocity();
+    joint_acceleration_ = trapezoidalTrajectory_->getAcceleration();
+    
+    // 更新时间
+    if (trapezoidalTrajectory_->isFinished(elapsed_time))
    {
-        // 计算当前时间
-        double current_time = i * time_step;
-
-        // 计算每个关节的当前位置
-        for (size_t j = 0; j < total_joints_; j++)
-        {
-            joint_position_[j] = joint_position_desired[j] * current_time / duration;
-        }
+        elapsed_time = 0.0;
+    }
+    else
+    {
+        elapsed_time += 0.01;  // 假设控制周期为10ms
    }
 }

+bool ArmControl::GoHome(std::vector<double>& joint_positions, double dt)
+{
+    bool all_joints_at_home = true;  // 标记是否所有关节都在零点
+    const double home_tolerance = 1e-3;  // 零点位置容差（可根据实际精度调整）
+
+    static double elapsed_time = 0.0;
+
+    // 若为新任务或关节位置已达目标，重新初始化轨迹
+    if (!is_moving_)
+    {
+        // 检查是否已经在零点
+        for (size_t i = 0; i < joint_home_positions_.size(); i++)
+        {
+            double position_error = std::fabs(joint_position_[i] - joint_home_positions_[i]);
+        
+            // 若误差超过容差，说明该关节不在零点
+            if (position_error > home_tolerance)
+            {
+                all_joints_at_home = false;  // 标记为未全部到达
+                break;
+            }
+            
+        }
+
+        if (all_joints_at_home)
+        {
+            joint_positions = joint_home_positions_;
+
+            return true; // 如果所有关节都在零点，则无需移动
+        }
+        
+        // 从当前关节位置开始规划轨迹
+        trapezoidalTrajectory_->init(joint_position_, joint_home_positions_);
+        elapsed_time = 0.0;       // 重置累计时间
+        is_moving_ = true;      // 标记为正在执行任务
+    }
+
+    // 使用梯形轨迹规划器计算当前时间点的目标位置
+    std::vector<double> desired_pos = trapezoidalTrajectory_->update(elapsed_time);
+
+    // 推进时间（累加控制周期）
+    elapsed_time += dt;
+
+    // 检查是否到达目标位置
+    if (trapezoidalTrajectory_->isFinished(elapsed_time))
+    {
+        //最后一点直接将终点设为目标点
+        joint_positions = joint_home_positions_;
+        
+        // 标记任务完成，下次调用视为新任务
+        is_moving_ = false;
+        elapsed_time = 0.0;
+
+        return true;
+    }
+    else
+    {
+        // 更新关节状态
+        joint_position_desired_ = desired_pos;
+        joint_velocity_ = trapezoidalTrajectory_->getVelocity();
+        joint_acceleration_ = trapezoidalTrajectory_->getAcceleration();
+
+        // 将计算出的位置通过输出参数返回
+        joint_positions = desired_pos;
+    }
+
+    return false;
+}
+
+bool ArmControl::Stop(std::vector<double>& joint_positions, double dt)
+{    
+    if (is_moving_ && !is_stopping_)
+    {
+        is_stopping_ = true;
+
+        // 从当前关节位置开始规划轨迹
+        trapezoidalTrajectory_->StopCalculate();
+    }
+
+    if (is_stopping_)
+    {
+        static double elapsed_time = 0.0;
+
+        std::vector<double> desired_pos = trapezoidalTrajectory_->StopUpdate(dt);
+
+        elapsed_time += dt;
+
+        if (trapezoidalTrajectory_->stopFinished(elapsed_time))
+        {
+            joint_positions = joint_position_;
+            is_moving_ = false;
+            is_stopping_ = false;
+            elapsed_time = 0.0;
+
+            return true;
+        }
+
+        joint_positions = desired_pos;
+
+        return false;
+    }
+
+    return true;    // 如果没有移动或停止，直接返回true
+}
+
+void ArmControl::UpdateJointStates(
+    const std::vector<double>& joint_positions,
+    const std::vector<double>& joint_velocities,
+    const std::vector<double>& joint_torques)
+{
+    // 2. 更新关节位置（同时同步关节角度，假设位置与角度单位一致）
+    joint_position_ = joint_positions;
+
+    // 3. 更新关节速度
+    joint_velocity_ = joint_velocities;
+
+    // 4. 更新关节力矩
+    joint_torque_ = joint_torques;
+}
+
+bool ArmControl::IsMoving()
+{
+    return is_moving_;
+}
+
+}  // namespace robot_control
--- a/HiveCoreR0/src/robot_control/src/input_device.cpp
+++ b/HiveCoreR0/src/robot_control/src/input_device.cpp
@@ -1,5 +1,6 @@
 #include "input_device.hpp"

+using namespace robot_control;

 // 键盘输入处理
 ControlCommand KeyboardInput::GetCommand() {
--- a/HiveCoreR0/src/robot_control/src/leg_control.cpp
+++ b/HiveCoreR0/src/robot_control/src/leg_control.cpp
@@ -1,42 +1,203 @@
 #include "leg_control.hpp"
+#include <stdexcept>
+#include <vector>

-using namespace robot_control;
-
-LegControl::LegControl(int totalJointNumber, double maxSpeed, double LegLength)
+namespace robot_control
 {
-    this->totalJointNumber_ = totalJointNumber;
-    this->maxSpeed_ = maxSpeed;
-    this->LegLength_ = LegLength;

-    this->joint_positions_.resize(totalJointNumber);
-}
-
-LegControl::~LegControl()
+LegControl::LegControl(
+    size_t totalJoints, 
+    double LegLength, 
+    double maxSpeed, 
+    double maxAcc, 
+    const std::vector<double>& minLimits, 
+    const std::vector<double>& maxLimits, 
+    const std::vector<double>& home_positions
+) : 
+    total_joints_(totalJoints),  
+    maxSpeed_(maxSpeed),                  
+    maxAcc_(maxAcc),                      
+    LegLength_(LegLength),                
+    joint_home_positions_(home_positions),
+    maxLimits_(maxLimits),                
+    minLimits_(minLimits)                 
 {
+    // 初始化关节状态向量
+    joint_position_.resize(total_joints_, 0.0);
+    joint_velocity_.resize(total_joints_, 0.0);
+    joint_acceleration_.resize(total_joints_, 0.0);
+    joint_torque_.resize(total_joints_, 0.0);
+    joint_position_desired_.resize(total_joints_, 0.0);
    
+    // 初始化梯形轨迹规划器
+    trapezoidalTrajectory_ = new TrapezoidalTrajectory(maxSpeed, maxAcc);
+
+    is_moving_ = false;
+
+    is_stopping_ = false;
 }

-void LegControl::setLegSpeed(double speed)
+LegControl::~LegControl() = default;
+
+bool LegControl::MoveToTargetJoint(std::vector<double>& joint_positions, const std::vector<double>& target_joint, double dt)
 {
-    if (speed > maxSpeed_)
+    if (joint_position_.size() != target_joint.size())
    {
-        speed = maxSpeed_;
+        throw std::invalid_argument("Joint position and target joint size do not match.");
    }
-    else if (speed < -maxSpeed_)
+    
+    static double elapsed_time = 0.0;
+
+    if (!is_moving_)
    {
-        speed = -maxSpeed_;
+        bool all_joints_reached = true;  
+        const double home_tolerance = 1e-3; 
+
+        for (size_t i = 0; i < target_joint.size(); i++)
+        {
+            double position_error = std::fabs(joint_position_[i] - target_joint[i]);
+            if (position_error > home_tolerance)
+            {
+                all_joints_reached = false;  
+                break;
+            }
+        }
+
+        if (all_joints_reached)
+        {
+            joint_positions = target_joint;
+
+            return true; 
+        }
+        
+        // 从当前关节位置开始规划轨迹
+        trapezoidalTrajectory_->init(joint_position_, target_joint);
+        elapsed_time = 0.0;       
+        is_moving_ = true;     
+        
+        std::cout << "Leg Moving to target joint! " << std::endl;
    }

-    this->speed_ = speed;
+    std::vector<double> desired_pos = trapezoidalTrajectory_->update(elapsed_time);
+    elapsed_time += dt;
+
+    // 检查是否到达目标位置
+    if (trapezoidalTrajectory_->isFinished(elapsed_time))
+    {
+        joint_positions = target_joint;
+        is_moving_ = false;
+        elapsed_time = 0.0;
+
+        std::cout << "Leg moving finished" << std::endl;
+        return true;
+    }
+    else
+    {
+        joint_velocity_ = trapezoidalTrajectory_->getVelocity();
+        joint_acceleration_ = trapezoidalTrajectory_->getAcceleration();
+
+        joint_positions = desired_pos;
+    }
+
+    return false;
 }

-void LegControl::Move(std::vector<double>& deltaJointPositions, double dt_sec)
+bool LegControl::MoveUp(std::vector<double>& joint_positions, double dt)
+{    
+    if (!is_moving_)
+    {
+        for (size_t i = 0; i < joint_position_.size(); i++)
+        {
+            joint_position_desired_[i] = maxLimits_[i];
+        }
+    }
+
+    return MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
+}
+
+bool LegControl::MoveDown(std::vector<double>& joint_positions, double dt)
+{    
+    if (!is_moving_)
+    {
+        for (size_t i = 0; i < joint_position_.size(); i++)
+        {
+            joint_position_desired_[i] = minLimits_[i];
+        }
+    }
+
+    return MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
+}
+
+bool LegControl::Stop(std::vector<double>& joint_positions, double dt)
+{    
+    if (is_moving_ && !is_stopping_)
+    {
+        is_stopping_ = true;
+
+        // 从当前关节位置开始规划轨迹
+        trapezoidalTrajectory_->StopCalculate();
+
+        std::cout << "Leg stopping " << std::endl;
+    }
+
+    if (is_stopping_)
+    {
+        static double elapsed_time = 0.0;
+
+        std::vector<double> desired_pos = trapezoidalTrajectory_->StopUpdate(dt);
+
+        elapsed_time += dt;
+
+        if (trapezoidalTrajectory_->stopFinished(elapsed_time))
+        {
+            joint_positions = joint_position_;
+            is_moving_ = false;
+            is_stopping_ = false;
+            elapsed_time = 0.0;
+            
+            std::cout << "Leg stopping finished" << std::endl;
+            return true;
+        }
+
+        joint_positions = desired_pos;
+
+        return false;
+    }
+
+    return true;
+}
+
+
+bool LegControl::GoHome(std::vector<double>& joint_positions, double dt)
 {
-    for (int i = 0; i < this->totalJointNumber_; i++)
+    if (!is_moving_)
    {
-        joint_positions_[i] = speed_ * dt_sec;
+        joint_position_desired_ = joint_home_positions_;
    }

-    deltaJointPositions = joint_positions_;
+    return MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
 }

+
+void LegControl::UpdateJointStates(
+    const std::vector<double>& joint_positions,
+    const std::vector<double>& joint_velocities,
+    const std::vector<double>& joint_torques)
+{
+    // 2. 更新关节位置（同时同步关节角度，假设位置与角度单位一致）
+    joint_position_ = joint_positions;
+
+    // 3. 更新关节速度
+    joint_velocity_ = joint_velocities;
+
+    // 4. 更新关节力矩
+    joint_torque_ = joint_torques;
+}
+
+bool LegControl::IsMoving()
+{
+    return is_moving_;
+}
+
+
+}  // namespace robot_control
--- a/HiveCoreR0/src/robot_control/src/main.cpp
+++ b/HiveCoreR0/src/robot_control/src/main.cpp
@@ -0,0 +1,24 @@
+#include <rclcpp/rclcpp.hpp>
+#include "robot_fsm.hpp"
+
+/**
+ * @brief 程序入口函数
+ * 功能：初始化ROS 2，创建状态机节点，启动自旋
+ */
+
+ using namespace robot_control;
+
+int main(int argc, char *argv[]) {
+    // 初始化ROS 2
+    rclcpp::init(argc, argv);
+
+    // 创建状态机节点（智能指针管理）
+    auto robot_fsm_node = std::make_shared<RobotFsm>();
+
+    // 启动节点自旋（阻塞，处理回调和定时器）
+    rclcpp::spin(robot_fsm_node);
+
+    // 关闭ROS 2
+    rclcpp::shutdown();
+    return 0;
+}
--- a/HiveCoreR0/src/robot_control/src/plot_joint_trajectory.py
+++ b/HiveCoreR0/src/robot_control/src/plot_joint_trajectory.py
@@ -0,0 +1,40 @@
+# 文件名：plot_joint_trajectory.py
+import numpy as np
+import matplotlib.pyplot as plt
+import os
+
+# 读取txt数据
+file_path = "/home/demo/ros2_joint_data.txt"
+if not os.path.exists(file_path):
+    print(f"文件不存在: {file_path}")
+    exit()
+
+# 解析数据（跳过表头）
+times = []
+positions = []
+with open(file_path, 'r') as f:
+    next(f)  # 跳过第一行"time,position"
+    for line in f:
+        line = line.strip()
+        if not line:
+            continue
+        t, pos = line.split(',')
+        times.append(float(t))
+        positions.append(float(pos))
+
+# 转换为相对时间（以第一个数据点为起点）
+if len(times) == 0:
+    print("无数据可绘制")
+    exit()
+start_time = times[0]
+relative_times = [t - start_time for t in times]
+
+# 绘制曲线
+plt.figure(figsize=(10, 6))
+plt.plot(positions, label='Joint 1 Position', color='b', linewidth=1.5)
+plt.xlabel('Time (s)')
+plt.ylabel('Position (rad)')  # 假设单位是弧度，根据实际情况修改
+plt.title('Joint Trajectory Curve')
+plt.grid(True, linestyle='--', alpha=0.7)
+plt.legend()
+plt.show()
--- a/HiveCoreR0/src/robot_control/src/plot_joint_trajectpry_multi.py
+++ b/HiveCoreR0/src/robot_control/src/plot_joint_trajectpry_multi.py
@@ -0,0 +1,65 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import os
+
+def plot_joint_trajectories(file_path):
+    # 检查文件是否存在
+    if not os.path.exists(file_path):
+        print(f"错误：文件不存在 - {file_path}")
+        return
+
+    # 读取数据
+    timestamps = []
+    joint_data = []  # 存储所有关节数据，格式：[ [关节1数据], [关节2数据], ... ]
+
+    with open(file_path, 'r') as f:
+        for line in f:
+            line = line.strip()
+            if not line:
+                continue
+            
+            # 解析一行数据（时间戳 + 所有关节值）
+            parts = list(map(float, line.split(',')))
+            timestamp = parts[0]
+            joints = parts[1:]  # 第0个是时间戳，后面是关节数据
+            
+            timestamps.append(timestamp)
+            
+            # 初始化关节数据列表（首次读取时）
+            if not joint_data:
+                joint_data = [[] for _ in range(len(joints))]
+            
+            # 按关节索引存储数据
+            for i, val in enumerate(joints):
+                if i < len(joint_data):  # 避免索引越界
+                    joint_data[i].append(val)
+
+    # 转换为相对时间（以第一个时间戳为起点）
+    if not timestamps:
+        print("警告：未读取到有效数据")
+        return
+    start_time = timestamps[0]
+    relative_times = [t - start_time for t in timestamps]
+
+    # 绘制所有关节轨迹（最多显示12个关节，避免图过于拥挤）
+    num_joints = len(joint_data)
+    num_plots = min(num_joints, 12)  # 超过12个关节只显示前12个
+
+    plt.figure(figsize=(12, 8))
+    for i in range(num_plots):
+        plt.plot(relative_times, joint_data[i], label=f'关节 {i+1}')
+
+    # 图形配置
+    plt.xlabel('时间 (秒)')
+    plt.ylabel('关节位置')
+    plt.title('所有关节轨迹曲线')
+    plt.grid(True, linestyle='--', alpha=0.7)
+    plt.legend(bbox_to_anchor=(1.05, 1), loc='upper left')  # 图例放右侧
+    plt.tight_layout()  # 自动调整布局
+    plt.show()
+
+if __name__ == "__main__":
+    # 数据文件路径（与C++代码中的路径一致）
+    data_file = "/home/demo/ros2_joint_data.txt"
+    plot_joint_trajectories(data_file)
+    
--- a/HiveCoreR0/src/robot_control/src/robot_control_manager.cpp
+++ b/HiveCoreR0/src/robot_control/src/robot_control_manager.cpp
@@ -1,6 +1,8 @@
 #include "robot_control_manager.hpp"

 using namespace robot_control;
+using namespace std;
+using namespace std_msgs::msg;


 RobotControlManager::RobotControlManager()
@@ -10,177 +12,311 @@ RobotControlManager::RobotControlManager()

 void RobotControlManager::init()
 {
+    robotInited_ = false;
+
    // Initialize the robot control manager
-    std::cout << "Initializing robot control manager..." << std::endl;
+    cout << "Initializing robot control manager..." << std::endl;

    // Initialize the robot control manager
    motionParams_ = MotionParameters();

    // Initialize the joint commands
-    for (int i = 0; i < motionParams_.total_joints_; i++)
+    for (size_t i = 0; i < motionParams_.total_joints_; i++)
    {
        jointCommands_.data.push_back(0.0);
    }

-    armController_ = new ArmControl(motionParams_.arm_joint_indices_.size());
+    leftArmController_ = new ArmControl(
+        motionParams_.left_arm_joint_indices_.size(), 
+        motionParams_.arm_length_l1_, 
+        motionParams_.arm_length_l2_,
+        motionParams_.max_joint_velocity_,
+        motionParams_.max_joint_acceleration_,
+        motionParams_.left_arm_min_limit_,
+        motionParams_.left_arm_max_limit_,
+        motionParams_.left_arm_home_positions_
+    );
+
+    rightArmController_ = new ArmControl(
+        motionParams_.right_arm_joint_indices_.size(), 
+        motionParams_.arm_length_l1_, 
+        motionParams_.arm_length_l2_,
+        motionParams_.max_joint_velocity_,
+        motionParams_.max_joint_acceleration_,
+        motionParams_.right_arm_min_limit_,
+        motionParams_.right_arm_max_limit_,
+        motionParams_.right_arm_home_positions_
+    );
+
+    leftLegController_ = new LegControl(
+        motionParams_.left_leg_joint_indices_.size(), 
+        motionParams_.leg_length_,
+        motionParams_.max_joint_velocity_,
+        motionParams_.max_joint_acceleration_,
+        motionParams_.left_leg_min_limit_,
+        motionParams_.left_leg_max_limit_,
+        motionParams_.left_leg_home_positions_
+    );
+
+    rightLegController_ = new LegControl(
+        motionParams_.right_leg_joint_indices_.size(), 
+        motionParams_.leg_length_,
+        motionParams_.max_joint_velocity_,
+        motionParams_.max_joint_acceleration_,
+        motionParams_.right_leg_min_limit_,
+        motionParams_.right_leg_max_limit_,
+        motionParams_.right_leg_home_positions_
+    );

-    legController_ = new LegControl(motionParams_.leg_joint_indices_.size(), motionParams_.max_linear_speed_z_, motionParams_.leg_length_);

    wheelController_ = new WheelControl(
        motionParams_.wheel_joint_indices_.size(), 
        motionParams_.wheel_radius_, 
        motionParams_.wheel_separation_, 
        motionParams_.max_linear_speed_x_, 
-        motionParams_.max_angular_speed_z_
+        motionParams_.max_angular_speed_z_,
+        motionParams_.wheel_joint_directions_
    );

-    MoveCounter_ = 0;
-    MoveTime_ = 0.0;
+    robotHomed_.resize(4, false);
 }

 RobotControlManager::~RobotControlManager()
 {
-    // Clean up the robot control manager
-    this->cleanup();
-    delete armController_;
-    delete legController_;
+    delete leftArmController_;
+    delete rightArmController_;
+    delete leftLegController_;
+    delete rightLegController_;
    delete wheelController_;
-    delete robotModel_;
 }

-void RobotControlManager::cleanup()
+Float64MultiArray RobotControlManager::GetJointCommands()
 {
-    // Clean up the robot control manager
-    std::cout << "Cleaning up robot control manager..." << std::endl;
+    Float64MultiArray tempValues;
+    tempValues.data.resize(motionParams_.total_joints_);

-}
-
-void RobotControlManager::UpdateJointStates(sensor_msgs::msg::JointState joint_states)
-{
-    // Update the joint states
-    jointStates_ = joint_states;
-}
-
-std_msgs::msg::Float64MultiArray RobotControlManager::GetJointCommands()
-{
-    // Get the joint commands
-    return jointCommands_;
-}
-
-void RobotControlManager::SetJointCommands(std_msgs::msg::Float64MultiArray joint_commands)
-{
-    // Set the joint commands
-    jointCommands_ = joint_commands;
-}
-
-void RobotControlManager::ResetJoints()
-{
-    // Reset the joints
-    std::cout << "Resetting joints..." << std::endl;
-    for (int i = 0; i < motionParams_.total_joints_; i++)
+    // Convert the joint commands to motor values and limit to encoder range
+    for (size_t i = 0; i < jointCommands_.data.size(); i++)
    {
-        jointCommands_.data[i] = 0.0;
+        double angle = jointCommands_.data[i];
+
+        double normalizedAngle = fmod(angle, 360.0);
+
+        // 计算原始脉冲值
+        double pulseValue = normalizedAngle * motionParams_.degree_to_pulse_;
+        
+        tempValues.data[i] = pulseValue;
+    }
+
+    return tempValues;
+}
+
+
+void RobotControlManager::CheckJointLimits(Float64MultiArray &tempJointValues)
+{
+    // Check the joint limits
+    for (size_t i = 0; i < tempJointValues.data.size(); i++)
+    {
+        if (tempJointValues.data[i] > motionParams_.joint_limits_[i].max)
+        {
+            tempJointValues.data[i] = motionParams_.joint_limits_[i].max;
+        }
+        
+        if (tempJointValues.data[i] < motionParams_.joint_limits_[i].min)
+        {
+            tempJointValues.data[i] = motionParams_.joint_limits_[i].min;
+        }
    }
 }

-void RobotControlManager::MoveForward(double dt)
+void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::SharedPtr msg) 
 {
-    // Move forward
-    wheelController_ -> setLinearAngularSpeed(motionParams_.linear_speed_x_, 0);
+    std::cout << "Update joint states" << std::endl;
+    jointStates_ = *msg;
+    jointPositions_.resize(motionParams_.total_joints_);
+    jointVelocities_.resize(motionParams_.total_joints_);
+    jointEfforts_.resize(motionParams_.total_joints_);

-    std::vector<double> tempWheelCmd;
-    wheelController_ -> Move(tempWheelCmd, dt);
-
-    for (int i = 0; i < motionParams_.wheel_joint_indices_.size(); i++)
+    for (size_t i = 0; i < motionParams_.total_joints_; i++)
    {
-        jointCommands_.data[motionParams_.wheel_joint_indices_[i]] += motionParams_.wheel_joint_directions_[i] * tempWheelCmd[i];
+        jointPositions_[i] = jointStates_.position[i] * motionParams_.pulse_to_degree_;    // convert to degree
+        jointVelocities_[i] = 0; //jointStates_.velocity[i] * motionParams_.pulse_to_degree_;
+        jointEfforts_[i] = 0; //jointStates_.effort[i] ; // TODO: check the factory
+
    }
+
+    std::cout << "Joint positions: ";
+
+    // TODO: This block can be deleted
+    if(!robotInited_)
+    {
+        robotInited_ = true;
+        for (size_t i = 0; i < motionParams_.total_joints_; i++)
+        {
+            jointCommands_.data[i] = jointStates_.position[i]* motionParams_.pulse_to_degree_;
+            std::cout << "Robot inited joint " << i + 1 <<  jointCommands_.data[i] << std::endl;
+        }
+        return;
+    }
+
+    // // Update the arm controller
+    // size_t armJointSize = motionParams_.left_arm_joint_indices_.size();
+    // std::vector<double> armPositions(jointPositions_.begin() + motionParams_.left_arm_joint_indices_[0], jointPositions_.begin() + motionParams_.left_arm_joint_indices_[0] + armJointSize);
+    // std::vector<double> armVelocities(jointVelocities_.begin() + motionParams_.left_arm_joint_indices_[0], jointVelocities_.begin() + motionParams_.left_arm_joint_indices_[0] + armJointSize);
+    // std::vector<double> armEfforts(jointEfforts_.begin() + motionParams_.left_arm_joint_indices_[0], jointEfforts_.begin() + motionParams_.left_arm_joint_indices_[0] + armJointSize);
+    // leftArmController_->UpdateJointStates(armPositions, armVelocities, armEfforts);
+
+    // armPositions = std::vector<double>(jointPositions_.begin() + motionParams_.right_arm_joint_indices_[0], jointPositions_.begin() + motionParams_.right_arm_joint_indices_[0] + armJointSize);
+    // armVelocities = std::vector<double>(jointVelocities_.begin() + motionParams_.right_arm_joint_indices_[0], jointVelocities_.begin() + motionParams_.right_arm_joint_indices_[0] + armJointSize);
+    // armEfforts = std::vector<double>(jointEfforts_.begin() + motionParams_.right_arm_joint_indices_[0], jointEfforts_.begin() + motionParams_.right_arm_joint_indices_[0] + armJointSize);
+    // rightArmController_->UpdateJointStates(armPositions, armVelocities, armEfforts);
+
+    // Update the leg controller
+    size_t legJointSize = motionParams_.left_leg_joint_indices_.size();
+    std::vector<double> legPositions(jointPositions_.begin() + motionParams_.left_leg_joint_indices_[0], jointPositions_.begin() + motionParams_.left_leg_joint_indices_[0] + legJointSize);
+    std::vector<double> legVelocities(jointVelocities_.begin() + motionParams_.left_leg_joint_indices_[0], jointVelocities_.begin() + motionParams_.left_leg_joint_indices_[0] + legJointSize);
+    std::vector<double> legEfforts(jointEfforts_.begin() + motionParams_.left_leg_joint_indices_[0], jointEfforts_.begin() + motionParams_.left_leg_joint_indices_[0] + legJointSize);
+    leftLegController_->UpdateJointStates(legPositions, legVelocities, legEfforts);
+
+    legPositions = std::vector<double>(jointPositions_.begin() + motionParams_.right_leg_joint_indices_[0], jointPositions_.begin() + motionParams_.right_leg_joint_indices_[0] + legJointSize);
+    legVelocities = std::vector<double>(jointVelocities_.begin() + motionParams_.right_leg_joint_indices_[0], jointVelocities_.begin() + motionParams_.right_leg_joint_indices_[0] + legJointSize);
+    legEfforts = std::vector<double>(jointEfforts_.begin() + motionParams_.right_leg_joint_indices_[0], jointEfforts_.begin() + motionParams_.right_leg_joint_indices_[0] + legJointSize);
+    rightLegController_->UpdateJointStates(legPositions, legVelocities, legEfforts);
 }

-void RobotControlManager::MoveBackward(double dt)
+MotionParameters RobotControlManager::GetMotionParameters()
 {
-    // Move forward
-    wheelController_ -> setLinearAngularSpeed( - motionParams_.linear_speed_x_, 0.0);
-
-    std::vector<double> tempDeltaWheelCmd;
-    wheelController_ -> Move(tempDeltaWheelCmd, dt);
-
-    for (int i = 0; i < motionParams_.wheel_joint_indices_.size(); i++)
-    {
-        jointCommands_.data[motionParams_.wheel_joint_indices_[i]] += motionParams_.wheel_joint_directions_[i] * tempDeltaWheelCmd[i];
-    }
+    return motionParams_;
 }

-void RobotControlManager::TurnLeft(double dt)
+void RobotControlManager::MoveForward()
 {
-    // Turn left
-    wheelController_ -> setLinearAngularSpeed(0.0, - motionParams_.angular_speed_z_);
-
-    std::vector<double> tempDeltaWheelCmd;
-    wheelController_ -> Move(tempDeltaWheelCmd, dt);
-
-    for (int i = 0; i < motionParams_.wheel_joint_indices_.size(); i++)
-    {
-        jointCommands_.data[motionParams_.wheel_joint_indices_[i]] += motionParams_.wheel_joint_directions_[i] * tempDeltaWheelCmd[i];
-    }
+    wheelController_ -> setLinearAngularSpeed(motionParams_.max_linear_speed_x_, 0);
+    wheelController_ -> ExecuteCommand();
 }

-void RobotControlManager::TurnRight(double dt)
+void RobotControlManager::MoveBackward()
 {
-    // Turn right
-    wheelController_ -> setLinearAngularSpeed(0.0, motionParams_.angular_speed_z_);
-
-    std::vector<double> tempDeltaWheelCmd;
-    wheelController_ -> Move(tempDeltaWheelCmd, dt);
-
-    for (int i = 0; i < motionParams_.wheel_joint_indices_.size(); i++)
-    {
-        jointCommands_.data[motionParams_.wheel_joint_indices_[i]] += motionParams_.wheel_joint_directions_[i] * tempDeltaWheelCmd[i];
-    }
+    wheelController_ -> setLinearAngularSpeed( - motionParams_.max_linear_speed_x_, 0.0);
+    wheelController_ -> ExecuteCommand();
 }

-void RobotControlManager::Stop()
+void RobotControlManager::TurnLeft()
+{
+    wheelController_ -> setLinearAngularSpeed(0.0, - motionParams_.max_angular_speed_z_);
+    wheelController_ -> ExecuteCommand();
+}
+
+void RobotControlManager::TurnRight()
+{
+    wheelController_ -> setLinearAngularSpeed(0.0, motionParams_.max_angular_speed_z_);
+    wheelController_ -> ExecuteCommand();
+}
+
+void RobotControlManager::Stop(double dt)
 {
-    // Stop
    wheelController_ -> setLinearAngularSpeed(0.0, 0.0);
+    wheelController_ -> ExecuteCommand();

-    std::vector<double> tempDeltaWheelCmd;
-    wheelController_ -> Move(tempDeltaWheelCmd, 0.0);
+    leftLegController_->Stop(tempLeftLegCmd, dt);
+
+    rightLegController_->Stop(tempRightLegCmd, dt);
+
+    leftArmController_->Stop(tempLeftArmCmd, dt);
+
+    rightArmController_->Stop(tempRightArmCmd, dt);
+
+    for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++)
+    {
+        jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = motionParams_.left_leg_joint_directions_[i] * tempLeftLegCmd[i];
+    }
+    for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++)
+    {
+        jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = motionParams_.right_leg_joint_directions_[i] * tempRightLegCmd[i];
+    }
+    for (size_t i = 0; i < motionParams_.left_arm_joint_indices_.size(); i++)
+    {
+        jointCommands_.data[motionParams_.left_arm_joint_indices_[i]] = motionParams_.left_arm_joint_directions_[i] * tempLeftArmCmd[i];
+    }
+    for (size_t i = 0; i < motionParams_.right_arm_joint_indices_.size(); i++)
+    {
+        jointCommands_.data[motionParams_.right_arm_joint_indices_[i]] = motionParams_.right_arm_joint_directions_[i] * tempRightArmCmd[i];
+    }
 }

 void RobotControlManager::MoveUp(double dt)
 {
-    legController_ -> setLegSpeed(motionParams_.linear_speed_z_);
+    leftLegController_ -> MoveUp(tempLeftLegCmd, dt);
+    rightLegController_ -> MoveUp(tempRightLegCmd, dt);

-    std::vector<double> tempDeltaLegCmd;
+    wheelController_-> setLinearAngularSpeed(5.0, 0.0); //TODO: value need to be changed
+    wheelController_ -> ExecuteCommand();

-    legController_ -> Move(tempDeltaLegCmd, dt);
-
-    for (int i = 0; i < motionParams_.leg_joint_indices_.size(); i++)
+    for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++)
    {
-        jointCommands_.data[motionParams_.leg_joint_indices_[i]] -= motionParams_.leg_joint_directions_[i] * tempDeltaLegCmd[i];
+        jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = motionParams_.left_leg_joint_directions_[i] * tempLeftLegCmd[i];
    }
-
-    for (int i = 0; i < motionParams_.wheel_joint_indices_.size(); i++)
+    for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++)
    {
-        jointCommands_.data[motionParams_.wheel_joint_indices_[i]] -=  motionParams_.leg_wheel_directions_[i] * tempDeltaLegCmd[i];
+        jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = motionParams_.right_leg_joint_directions_[i] * tempRightLegCmd[i];
    }
 }

 void RobotControlManager::MoveDown(double dt)
 {
-    legController_ -> setLegSpeed( - motionParams_.linear_speed_z_);
+    leftLegController_ -> MoveDown(tempLeftLegCmd, dt);
+    rightLegController_ -> MoveDown(tempRightLegCmd, dt);

-    std::vector<double> tempDeltaLegCmd;
-    legController_ -> Move(tempDeltaLegCmd, dt);
+    wheelController_-> setLinearAngularSpeed(-5.0, 0.0); //TODO: value need to be changed
+    wheelController_ -> ExecuteCommand();

-    for (int i = 0; i < motionParams_.leg_joint_indices_.size(); i++)
+    for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++)
    {
-        jointCommands_.data[motionParams_.leg_joint_indices_[i]] -= motionParams_.leg_joint_directions_[i] * tempDeltaLegCmd[i];
+        jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = motionParams_.left_leg_joint_directions_[i] * tempLeftLegCmd[i];
+    }
+    for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++)
+    {
+        jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = motionParams_.right_leg_joint_directions_[i] * tempRightLegCmd[i];
+    }
+}
+
+bool RobotControlManager::GoHome(double dt) 
+{
+    if(!robotHomed_[0])
+    {
+        robotHomed_[0] = leftArmController_->GoHome(tempLeftArmCmd, dt);
+        for (size_t i = 0; i < motionParams_.left_arm_joint_indices_.size(); i++)
+        {
+            jointCommands_.data[motionParams_.left_arm_joint_indices_[i]] = tempLeftArmCmd[i];
+        }
    }

-    for (int i = 0; i < motionParams_.wheel_joint_indices_.size(); i++)
+    if(!robotHomed_[1])
    {
-        jointCommands_.data[motionParams_.wheel_joint_indices_[i]] -=  motionParams_.leg_wheel_directions_[i] * tempDeltaLegCmd[i];
+        robotHomed_[1] = rightArmController_->GoHome(tempRightArmCmd, dt);
+        for (size_t i = 0; i < motionParams_.right_arm_joint_indices_.size(); i++)
+        {
+            jointCommands_.data[motionParams_.right_arm_joint_indices_[i]] = tempRightArmCmd[i];
+        }
    }
-}
+
+    // if(!robotHomed_[2])
+    // {
+    //     robotHomed_[2] = leftLegController_->GoHome(tempLeftLegCmd, dt);
+    //     for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++)
+    //     {
+    //         jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = tempLeftLegCmd[i];
+    //     }
+    // }
+
+    // if(!robotHomed_[3])
+    // {
+    //     robotHomed_[3] = rightLegController_->GoHome(tempRightLegCmd, dt);
+    //     for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++)
+    //     {
+    //         jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = tempRightLegCmd[i];
+    //     }
+    // }
+
+    return robotHomed_[0] && robotHomed_[1];// && homeFinished[2] && homeFinished[3];
+}
--- a/HiveCoreR0/src/robot_control/src/robot_fsm.cpp
+++ b/HiveCoreR0/src/robot_control/src/robot_fsm.cpp
@@ -1,12 +1,35 @@
 #include "rclcpp/rclcpp.hpp"
 #include "robot_fsm.hpp"

+using namespace robot_control;
+
+namespace fs = std::filesystem;

 // 机器人控制器构造函数
-RobotFsm::RobotFsm() : Node("keyboard_control_node"){
+RobotFsm::RobotFsm() : Node("robot_Fsm_node"){

    InitTransitionTable();
-    executionTime_ = 0.0;
+    currentState_ = RobotState::STOPPED;
+    previousState_ = RobotState::STOPPED;
+
+    isRobotEnabled_ = false;
+
+    // 初始化数据文件（设置路径，确保目录存在）
+    data_file_path_ = "/home/demo/ros2_joint_data.txt";  // 数据保存路径
+    fs::path file_path(data_file_path_);
+    fs::create_directories(file_path.parent_path());  // 自动创建父目录（如果不存在）
+
+    // 打开文件（追加模式，避免覆盖历史数据）
+    data_file_.open(data_file_path_, std::ios::trunc);
+    if (!data_file_.is_open()) {
+        
+    } else {
+        // 写入表头（仅在文件为空时）
+        data_file_.seekp(0, std::ios::end);  // 移动到文件末尾
+        if (data_file_.tellp() == 0) {  // 文件为空
+            data_file_ << "timestamp(sec),joint_1_position" << std::endl;
+        }
+    }

    // 添加输入设备
    AddInputDevice(InputType::KEYBOARD, std::make_unique<KeyboardInput>());
@@ -14,19 +37,22 @@ RobotFsm::RobotFsm() : Node("keyboard_control_node"){
    AddInputDevice(InputType::JOY, std::make_unique<JoyInput>());

     // 创建发布者
-    cmdPub_ = this->create_publisher<std_msgs::msg::Float64MultiArray>("joint_commands", 10);
+    cmdPub_ = this->create_publisher<trajectory_msgs::msg::JointTrajectory>("/trajectory_controller/joint_trajectory", 10);

-    // 订阅键盘输出节点数据
-    jointStatesSub_ = this->create_subscription<sensor_msgs::msg::JointState>("joint_states", 10,std::bind(&RobotFsm::JointStatesCallback, this, std::placeholders::_1));
+    // 创建发布者
+    gpioPub_ = this->create_publisher<control_msgs::msg::DynamicInterfaceGroupValues>("/gpio_command_controller/commands", 10);
+
+    // 订阅仿真状态
+    jointStatesSub_ = this->create_subscription<sensor_msgs::msg::JointState>("/joint_states", 10,std::bind(&RobotFsm::JointStatesCallback, this, std::placeholders::_1));

    // 创建订阅者（接收来自速度指令）
-    keyboadCommandSub_ = this->create_subscription<std_msgs::msg::String>("keyboard_command", 10,std::bind(&RobotFsm::KeyboardCommandCallback, this, std::placeholders::_1));
+    keyboadCommandSub_ = this->create_subscription<std_msgs::msg::String>("/keyboard_command", 10,std::bind(&RobotFsm::KeyboardCommandCallback, this, std::placeholders::_1));

    // 创建订阅者（接收来自语音指令）
-    voiceCommandSub_ = this->create_subscription<std_msgs::msg::String>("voice_command", 10,std::bind(&RobotFsm::VoiceCommandCallback, this, std::placeholders::_1));
+    voiceCommandSub_ = this->create_subscription<std_msgs::msg::String>("/voice_command", 10,std::bind(&RobotFsm::VoiceCommandCallback, this, std::placeholders::_1));

    // 创建订阅者（接收来自游戏手柄指令）
-    joyCommandSub_ = this->create_subscription<std_msgs::msg::String>("joy_command", 10,std::bind(&RobotFsm::JoyCommandCallback, this, std::placeholders::_1));
+    joyCommandSub_ = this->create_subscription<std_msgs::msg::String>("/joy_command", 10,std::bind(&RobotFsm::JoyCommandCallback, this, std::placeholders::_1));

    lastTime_ = this->now();  // 初始化时间

@@ -36,12 +62,20 @@ RobotFsm::RobotFsm() : Node("keyboard_control_node"){

 RobotFsm::~RobotFsm() 
 {
+    if (data_file_.is_open()) {
+        data_file_.close();  // 析构时关闭文件
+    }
    std::cout << "Robot controller stopped." << std::endl;
 }

 void RobotFsm::JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg) {
-    // 处理接收到的关节状态数据
-    // ...
+    if (!msg) {  // 检查消息是否有效
+        RCLCPP_WARN(this->get_logger(), "收到空的joint_states消息,忽略");
+        return;
+    }
+
+    // 更新机器人状态
+    robotControlManager_.UpdateJointStates(msg);
 }

 void RobotFsm::KeyboardCommandCallback(const std_msgs::msg::String::SharedPtr msg) {
@@ -88,31 +122,63 @@ void RobotFsm::JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg) {
    // 解析消息内容
    std::string command = msg->data;
    if (command == "前进") {
-        inputDevices_[InputType::VOICE]->SetCommand(ControlCommand::FORWARD);
+        inputDevices_[InputType::JOY]->SetCommand(ControlCommand::FORWARD);
    }
    else if (command == "后退") {
-        inputDevices_[InputType::VOICE]->SetCommand(ControlCommand::BACKWARD);
+        inputDevices_[InputType::JOY]->SetCommand(ControlCommand::BACKWARD);
    }
    else if (command == "左转") {
-        inputDevices_[InputType::VOICE]->SetCommand(ControlCommand::LEFT);
+        inputDevices_[InputType::JOY]->SetCommand(ControlCommand::LEFT);
    }
    else if (command == "右转") {
-        inputDevices_[InputType::VOICE]->SetCommand(ControlCommand::RIGHT);
+        inputDevices_[InputType::JOY]->SetCommand(ControlCommand::RIGHT);
    }
    else if (command == "上升") {
-        inputDevices_[InputType::VOICE]->SetCommand(ControlCommand::UP);
+        inputDevices_[InputType::JOY]->SetCommand(ControlCommand::UP);
    }
    else if (command == "下降") {
-        inputDevices_[InputType::VOICE]->SetCommand(ControlCommand::DOWN);
+        inputDevices_[InputType::JOY]->SetCommand(ControlCommand::DOWN);
    }
    else if (command == "停止") {
-        inputDevices_[InputType::VOICE]->SetCommand(ControlCommand::STOP);
+        inputDevices_[InputType::JOY]->SetCommand(ControlCommand::STOP);
+    }
+    else if (command == "A") {
+        inputDevices_[InputType::JOY]->SetCommand(ControlCommand::GO_HOME);
+    }
+    else if (command == "B") {
+        if (!isRobotEnabled_)
+        {
+            EnableRobot();
+        }
    }
    else {
        RCLCPP_WARN(this->get_logger(), "未知的命令: %s", command.c_str());
    }
 }

+void RobotFsm::EnableRobot() 
+{
+    control_msgs::msg::DynamicInterfaceGroupValues enableMsg;
+    
+    for (size_t i = 0; i < robotControlManager_.GetMotionParameters().total_joints_; i++)
+    {
+        std::string tempInterfaceGroup = "joint_" + std::to_string(i+1);
+        enableMsg.interface_groups.push_back(tempInterfaceGroup);
+
+        control_msgs::msg::InterfaceValue tempValue;
+        tempValue.interface_names = {"enable"};
+        tempValue.values = {1};
+        enableMsg.interface_values.push_back(tempValue);
+    }
+    
+    std::cout << "Enable robot" << std::endl;
+
+    // 发布消息
+    gpioPub_->publish(enableMsg);
+
+    isRobotEnabled_ = true;
+}
+
 void RobotFsm::VoiceCommandCallback(const std_msgs::msg::String::SharedPtr msg) {
    if (!msg) {  // 检查消息是否有效
        RCLCPP_WARN(this->get_logger(), "收到空的keyboard消息,忽略");
@@ -157,6 +223,7 @@ void RobotFsm::InitTransitionTable() {
    transitionTable_[{RobotState::TURNING_RIGHT, ControlCommand::STOP}] = RobotState::STOPPED;
    transitionTable_[{RobotState::MOVING_UP, ControlCommand::STOP}] = RobotState::STOPPED;
    transitionTable_[{RobotState::MOVING_DOWN, ControlCommand::STOP}] = RobotState::STOPPED;
+    transitionTable_[{RobotState::HOME_MOVING, ControlCommand::STOP}] = RobotState::STOPPED;
    
    // 从停止状态转换到其他状态
    transitionTable_[{RobotState::STOPPED, ControlCommand::FORWARD}] = RobotState::MOVING_FORWARD;
@@ -165,6 +232,7 @@ void RobotFsm::InitTransitionTable() {
    transitionTable_[{RobotState::STOPPED, ControlCommand::RIGHT}] = RobotState::TURNING_RIGHT;
    transitionTable_[{RobotState::STOPPED, ControlCommand::UP}] = RobotState::MOVING_UP;
    transitionTable_[{RobotState::STOPPED, ControlCommand::DOWN}] = RobotState::MOVING_DOWN;
+    transitionTable_[{RobotState::STOPPED, ControlCommand::GO_HOME}] = RobotState::HOME_MOVING;
 }

 // 添加输入设备
@@ -182,7 +250,7 @@ void RobotFsm::ControlLoop() {
            ControlCommand deviceCmd = device->GetCommand();
            if (deviceCmd != ControlCommand::NONE) {
                cmd = deviceCmd;
-                break;  // 优先级：先获取到的命令先执行
+                break;  
            }
        }
        
@@ -193,12 +261,81 @@ void RobotFsm::ControlLoop() {
        rclcpp::Duration dt = current_time - lastTime_;
        double dt_sec = dt.seconds();
        lastTime_ = current_time;
-
-        // 执行当前状态的动作
+        
        ExecuteStateAction(dt_sec);

-        auto cmd_msg = robotControlManager_.GetJointCommands();
-        cmdPub_->publish(cmd_msg);
+        if (isRobotEnabled_) {
+             Gpio_publish_joint_trajectory();
+        }
+        
+
+}
+
+void RobotFsm::Publish_joint_trajectory() 
+{
+    auto cmd_msg = robotControlManager_.GetJointCommands();
+    trajectory_msgs::msg::JointTrajectory traj_msg;
+    traj_msg.header.stamp = rclcpp::Time(0, 0);
+    traj_msg.header.frame_id = ""; 
+
+    //TODO: ADD MORE JOINTS
+    traj_msg.joint_names = {
+        "joint_1"
+    };
+
+    size_t JOINT_NUM = traj_msg.joint_names.size();
+
+    trajectory_msgs::msg::JointTrajectoryPoint traj_point;
+    std::vector<double> positions(JOINT_NUM);
+    for (size_t i = 0; i < JOINT_NUM; ++i) {
+        positions[i] = cmd_msg.data[14];  
+    }
+    
+    traj_point.positions = positions;
+    traj_point.time_from_start = rclcpp::Duration::from_seconds(1);
+
+    traj_msg.points.push_back(traj_point);
+
+    cmdPub_->publish(traj_msg);
+
+    // 串口类的用串口发送
+        
+}
+
+void RobotFsm::Gpio_publish_joint_trajectory() 
+{
+    auto cmd_msg = robotControlManager_.GetJointCommands();
+
+    // 1. 获取当前时间戳
+    rclcpp::Time current_time = this->now();
+    double timestamp = current_time.seconds();
+
+    // 2. 保存整个数组数据到txt文件
+    if (data_file_.is_open()) {
+        data_file_ << timestamp;
+
+        for (const auto& val : cmd_msg.data) {
+            data_file_ << "," << val;  // 用逗号分隔每个元素
+        }
+        
+        data_file_ << std::endl;  // 换行
+        data_file_.flush();  // 强制刷新
+    }
+
+    control_msgs::msg::DynamicInterfaceGroupValues positionMsg;
+    for (size_t i = 0; i < robotControlManager_.GetMotionParameters().total_joints_; i++)
+    {
+        std::string tempInterfaceGroup = "joint_" + std::to_string(i+1);
+        positionMsg.interface_groups.push_back(tempInterfaceGroup);
+
+        control_msgs::msg::InterfaceValue tempValue;
+        tempValue.interface_names = {"position"};
+        tempValue.values = {cmd_msg.data[i]};
+        positionMsg.interface_values.push_back(tempValue);
+    }
+    
+    // 发布消息
+    // gpioPub_->publish(positionMsg);   
 }

 // 处理命令并转换状态
@@ -209,7 +346,7 @@ void RobotFsm::ProcessCommand(ControlCommand cmd) {
    if (it != transitionTable_.end()) {
        RobotState newState = it->second;

-        if (newState != currentState_) {
+        if (newState != previousState_) {
            std::cout << "State changed: " << StateToString(currentState_) 
                      << " -> " << StateToString(newState) << std::endl;
            currentState_ = newState;
@@ -219,23 +356,22 @@ void RobotFsm::ProcessCommand(ControlCommand cmd) {

 // 执行当前状态对应的动作
 void RobotFsm::ExecuteStateAction(double dt) {
+
    switch (currentState_) {
        case RobotState::STOPPED:
-            robotControlManager_.Stop();
+            robotControlManager_.Stop(dt);
            break;
        case RobotState::MOVING_FORWARD:
-            robotControlManager_.MoveForward(dt);
+            robotControlManager_.MoveForward();
            break;
        case RobotState::MOVING_BACKWARD:
-            robotControlManager_.MoveBackward(dt);
+            robotControlManager_.MoveBackward();
            break;
        case RobotState::TURNING_LEFT:
-            robotControlManager_.TurnLeft(dt);
-            // 发送左转命令
+            robotControlManager_.TurnLeft();
            break;
        case RobotState::TURNING_RIGHT:
-            robotControlManager_.TurnRight(dt);
-            // 发送右转命令
+            robotControlManager_.TurnRight();
            break;
        case RobotState::MOVING_UP:
            robotControlManager_.MoveUp(dt);
@@ -244,7 +380,15 @@ void RobotFsm::ExecuteStateAction(double dt) {
            robotControlManager_.MoveDown(dt);
            // 发送下降命令
            break;
+        case RobotState::HOME_MOVING:
+            if(robotControlManager_.GoHome(dt))
+            {
+                inputDevices_[InputType::JOY]->SetCommand(ControlCommand::STOP);
+            }
+            break;
    }
+
+    previousState_ = currentState_;
 }

 // 状态转换为字符串
@@ -257,14 +401,7 @@ std::string RobotFsm::StateToString(RobotState state) const {
        case RobotState::TURNING_RIGHT: return "TURNING_RIGHT";
        case RobotState::MOVING_UP: return "MOVING_UP";
        case RobotState::MOVING_DOWN: return "MOVING_DOWN";
+        case RobotState::HOME_MOVING: return "HOME_MOVING";
        default: return "UNKNOWN";
    }
-}
-
-int main(int argc, char * argv[]) {
-    rclcpp::init(argc, argv);
-    auto node = std::make_shared<RobotFsm>();
-    rclcpp::spin(node);
-    rclcpp::shutdown();
-    return 0;
 }
--- a/HiveCoreR0/src/robot_control/src/robot_kinematics.cpp
+++ b/HiveCoreR0/src/robot_control/src/robot_kinematics.cpp
@@ -3,8 +3,7 @@
 #include <stdexcept>
 #include <iostream>

-namespace robot_control
-{
+using namespace robot_control;

 RobotKinematics::RobotKinematics(std::shared_ptr<RobotModel> robot_model)
    : robot_model_(robot_model)
@@ -212,4 +211,3 @@ bool RobotKinematics::leg_inverse_kinematics(size_t leg_index, const Eigen::Vect
    return true;
 }

-}  // namespace robot_model
--- a/HiveCoreR0/src/robot_control/src/robot_model.cpp
+++ b/HiveCoreR0/src/robot_control/src/robot_model.cpp
@@ -2,185 +2,183 @@
 #include <iostream>
 #include <cmath>

-namespace robot_control
+using namespace robot_control;
+
+RobotModel::RobotModel()
 {
+    initialize_model();
+}

-    RobotModel::RobotModel()
-    {
-        initialize_model();
-    }
+void RobotModel::initialize_model()
+{
+    // 初始化机械臂（4个关节，2个连杆）
+    // 添加关节
+    arms_[0].joints.push_back(std::make_shared<Joint>("arm_base_joint", -M_PI/2, M_PI/2, 10.0));
+    arms_[0].joints.push_back(std::make_shared<Joint>("arm_shoulder_joint", -M_PI/2, M_PI/2, 8.0));
+    arms_[0].joints.push_back(std::make_shared<Joint>("arm_elbow_joint", 0, M_PI/2, 6.0));
+    arms_[0].joints.push_back(std::make_shared<Joint>("arm_wrist_joint", -M_PI/4, M_PI/4, 4.0));
+    
+    // 添加连杆
+    arms_[0].links.push_back(std::make_shared<Link>("arm_upper_link", 0.3, 1.5));
+    arms_[0].links.push_back(std::make_shared<Link>("arm_lower_link", 0.25, 1.0));

-    void RobotModel::initialize_model()
+    // 初始化腿部（4条腿，每条腿2个连杆）
+    std::vector<std::string> leg_sides = {"front_left", "front_right", "rear_left", "rear_right"};
+    
+    for (const auto & side : leg_sides)
    {
-        // 初始化机械臂（4个关节，2个连杆）
-        // 添加关节
-        arms_[0].joints.push_back(std::make_shared<Joint>("arm_base_joint", -M_PI/2, M_PI/2, 10.0));
-        arms_[0].joints.push_back(std::make_shared<Joint>("arm_shoulder_joint", -M_PI/2, M_PI/2, 8.0));
-        arms_[0].joints.push_back(std::make_shared<Joint>("arm_elbow_joint", 0, M_PI/2, 6.0));
-        arms_[0].joints.push_back(std::make_shared<Joint>("arm_wrist_joint", -M_PI/4, M_PI/4, 4.0));
+        Leg leg;
+        leg.side = side;
        
-        // 添加连杆
-        arms_[0].links.push_back(std::make_shared<Link>("arm_upper_link", 0.3, 1.5));
-        arms_[0].links.push_back(std::make_shared<Link>("arm_lower_link", 0.25, 1.0));
-
-        // 初始化腿部（4条腿，每条腿2个连杆）
-        std::vector<std::string> leg_sides = {"front_left", "front_right", "rear_left", "rear_right"};
+        // 为每条腿添加关节
+        leg.joints.push_back(std::make_shared<Joint>(side + "_hip_joint", -M_PI/4, M_PI/4, 15.0));
+        leg.joints.push_back(std::make_shared<Joint>(side + "_knee_joint", 0, M_PI/2, 12.0));
        
-        for (const auto & side : leg_sides)
-        {
-            Leg leg;
-            leg.side = side;
-            
-            // 为每条腿添加关节
-            leg.joints.push_back(std::make_shared<Joint>(side + "_hip_joint", -M_PI/4, M_PI/4, 15.0));
-            leg.joints.push_back(std::make_shared<Joint>(side + "_knee_joint", 0, M_PI/2, 12.0));
-            
-            // 为每条腿添加连杆
-            leg.links.push_back(std::make_shared<Link>(side + "_thigh_link", 0.2, 2.0));
-            leg.links.push_back(std::make_shared<Link>(side + "_shin_link", 0.2, 1.5));
-            
-            legs_.push_back(leg);
-        }
+        // 为每条腿添加连杆
+        leg.links.push_back(std::make_shared<Link>(side + "_thigh_link", 0.2, 2.0));
+        leg.links.push_back(std::make_shared<Link>(side + "_shin_link", 0.2, 1.5));
+        
+        legs_.push_back(leg);
    }
+}

-    const Arm & RobotModel::get_arm(size_t index) const
+const Arm & RobotModel::get_arm(size_t index) const
+{
+    if (index >= arms_.size())
    {
-        if (index >= arms_.size())
-        {
-            throw std::out_of_range("Arm index out of range");
-        }
-        return arms_[index];
+        throw std::out_of_range("Arm index out of range");
    }
+    return arms_[index];
+}

-    const Leg & RobotModel::get_leg(size_t index) const
+const Leg & RobotModel::get_leg(size_t index) const
+{
+    if (index >= legs_.size())
    {
-        if (index >= legs_.size())
-        {
-            throw std::out_of_range("Leg index out of range");
-        }
-        return legs_[index];
+        throw std::out_of_range("Leg index out of range");
    }
+    return legs_[index];
+}

-    bool RobotModel::set_joint_angle(const std::string & joint_name, double angle)
+bool RobotModel::set_joint_angle(const std::string & joint_name, double angle)
+{
+    // 检查机械臂关节
+    for (auto & arm : arms_)
    {
-        // 检查机械臂关节
-        for (auto & arm : arms_)
+        for (const auto & joint : arm.joints)
        {
-            for (const auto & joint : arm.joints)
+            if (joint->name == joint_name)
            {
-                if (joint->name == joint_name)
+                // 检查角度限制
+                if (angle >= joint->min_angle && angle <= joint->max_angle)
                {
-                    // 检查角度限制
-                    if (angle >= joint->min_angle && angle <= joint->max_angle)
-                    {
-                        joint->angle = angle;
-                        return true;
-                    }
-                    return false;
+                    joint->angle = angle;
+                    return true;
                }
+                return false;
            }
        }
-        
-        // 检查腿部关节
-        for (auto & leg : legs_)
-        {
-            for (const auto & joint : leg.joints)
-            {
-                if (joint->name == joint_name)
-                {
-                    // 检查角度限制
-                    if (angle >= joint->min_angle && angle <= joint->max_angle)
-                    {
-                        joint->angle = angle;
-                        return true;
-                    }
-                    return false;
-                }
-            }
-        }
-        
-        // 未找到关节
-        return false;
    }
-
-    double RobotModel::get_joint_angle(const std::string & joint_name) const
+    
+    // 检查腿部关节
+    for (auto & leg : legs_)
    {
-        // 检查机械臂关节
-        for (auto & arm : arms_)
+        for (const auto & joint : leg.joints)
        {
-            for (const auto & joint : arm.joints)
+            if (joint->name == joint_name)
            {
-                if (joint->name == joint_name)
+                // 检查角度限制
+                if (angle >= joint->min_angle && angle <= joint->max_angle)
                {
-                    return joint->angle;
+                    joint->angle = angle;
+                    return true;
                }
+                return false;
            }
        }
-        
-        // 检查腿部关节
-        for (const auto & leg : legs_)
-        {
-            for (const auto & joint : leg.joints)
-            {
-                if (joint->name == joint_name)
-                {
-                    return joint->angle;
-                }
-            }
-        }
-        
-        // 未找到关节
-        return 0.0;
    }
+    
+    // 未找到关节
+    return false;
+}

-    void RobotModel::print_model_info() const
+double RobotModel::get_joint_angle(const std::string & joint_name) const
+{
+    // 检查机械臂关节
+    for (auto & arm : arms_)
    {
-        std::cout << "=== Robot Model Information ===" << std::endl;
-        
-        // 打印机械臂信息
-        std::cout << "\nArm:" << std::endl;
-        std::cout << "  Joints:" << std::endl;
-        for (size_t i = 0; i < arms_.size(); ++i)
+        for (const auto & joint : arm.joints)
        {
-            const auto & arm = arms_[i];
-            std::cout << "  Leg " << i << " (" << arm.side << "):" << std::endl;
-            std::cout << "    Joints:" << std::endl;
-            for (const auto & joint : arm.joints)
+            if (joint->name == joint_name)
            {
-                std::cout << "    " << joint->name << ": "
-                        << "Angle=" << joint->angle << " rad, "
-                        << "Range=[" << joint->min_angle << ", " << joint->max_angle << "] rad" << std::endl;
-            }
-            std::cout << "  Links:" << std::endl;
-            for (const auto & link : arm.links)
-            {
-                std::cout << "    " << link->name << ": "
-                        << "Length=" << link->length << " m, "
-                        << "Mass=" << link->mass << " kg" << std::endl;
-            }
-        }
-        
-        // 打印腿部信息
-        std::cout << "\nLegs:" << std::endl;
-        for (size_t i = 0; i < legs_.size(); ++i)
-        {
-            const auto & leg = legs_[i];
-            std::cout << "  Leg " << i << " (" << leg.side << "):" << std::endl;
-            std::cout << "    Joints:" << std::endl;
-            for (const auto & joint : leg.joints)
-            {
-                std::cout << "      " << joint->name << ": "
-                        << "Angle=" << joint->angle << " rad, "
-                        << "Range=[" << joint->min_angle << ", " << joint->max_angle << "] rad" << std::endl;
-            }
-            std::cout << "    Links:" << std::endl;
-            for (const auto & link : leg.links)
-            {
-                std::cout << "      " << link->name << ": "
-                        << "Length=" << link->length << " m, "
-                        << "Mass=" << link->mass << " kg" << std::endl;
+                return joint->angle;
            }
        }
    }
+    
+    // 检查腿部关节
+    for (const auto & leg : legs_)
+    {
+        for (const auto & joint : leg.joints)
+        {
+            if (joint->name == joint_name)
+            {
+                return joint->angle;
+            }
+        }
+    }
+    
+    // 未找到关节
+    return 0.0;
+}
+
+void RobotModel::print_model_info() const
+{
+    std::cout << "=== Robot Model Information ===" << std::endl;
+    
+    // 打印机械臂信息
+    std::cout << "\nArm:" << std::endl;
+    std::cout << "  Joints:" << std::endl;
+    for (size_t i = 0; i < arms_.size(); ++i)
+    {
+        const auto & arm = arms_[i];
+        std::cout << "  Leg " << i << " (" << arm.side << "):" << std::endl;
+        std::cout << "    Joints:" << std::endl;
+        for (const auto & joint : arm.joints)
+        {
+            std::cout << "    " << joint->name << ": "
+                    << "Angle=" << joint->angle << " rad, "
+                    << "Range=[" << joint->min_angle << ", " << joint->max_angle << "] rad" << std::endl;
+        }
+        std::cout << "  Links:" << std::endl;
+        for (const auto & link : arm.links)
+        {
+            std::cout << "    " << link->name << ": "
+                    << "Length=" << link->length << " m, "
+                    << "Mass=" << link->mass << " kg" << std::endl;
+        }
+    }
+    
+    // 打印腿部信息
+    std::cout << "\nLegs:" << std::endl;
+    for (size_t i = 0; i < legs_.size(); ++i)
+    {
+        const auto & leg = legs_[i];
+        std::cout << "  Leg " << i << " (" << leg.side << "):" << std::endl;
+        std::cout << "    Joints:" << std::endl;
+        for (const auto & joint : leg.joints)
+        {
+            std::cout << "      " << joint->name << ": "
+                    << "Angle=" << joint->angle << " rad, "
+                    << "Range=[" << joint->min_angle << ", " << joint->max_angle << "] rad" << std::endl;
+        }
+        std::cout << "    Links:" << std::endl;
+        for (const auto & link : leg.links)
+        {
+            std::cout << "      " << link->name << ": "
+                    << "Length=" << link->length << " m, "
+                    << "Mass=" << link->mass << " kg" << std::endl;
+        }
+    }
+}

-}  // namespace robot_model
--- a/HiveCoreR0/src/robot_control/src/rs485_driver.cpp
+++ b/HiveCoreR0/src/robot_control/src/rs485_driver.cpp
@@ -0,0 +1,384 @@
+#include "rs485_driver.hpp"
+#include <fcntl.h>
+#include <unistd.h>
+#include <cstring>
+#include <iostream>
+#include <chrono>
+#include <thread>
+
+namespace robot_control
+{
+
+    const unsigned char CRC8Table[]={  
+        0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,  
+        157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,  
+        35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128, 222, 60, 98,  
+        190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158, 29, 67, 161, 255,  
+        70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56, 102, 229, 187, 89, 7,  
+        219, 133, 103, 57, 186, 228, 6, 88, 25, 71, 165, 251, 120, 38, 196, 154,  
+        101, 59, 217, 135, 4, 90, 184, 230, 167, 249, 27, 69, 198, 152, 122, 36,  
+        248, 166, 68, 26, 153, 199, 37, 123, 58, 100, 134, 216, 91, 5, 231, 185,  
+        140, 210, 48, 110, 237, 179, 81, 15, 78, 16, 242, 172, 47, 113, 147, 205,  
+        17, 79, 173, 243, 112, 46, 204, 146, 211, 141, 111, 49, 178, 236, 14, 80,  
+        175, 241, 19, 77, 206, 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238,  
+        50, 108, 142, 208, 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115,  
+        202, 148, 118, 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139,  
+        87, 9, 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22,  
+        233, 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168,  
+        116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107, 53  
+    }; 
+    /**
+        *	@name		CRC8_Table
+    * @brief  This function is used to Get CRC8 check value.
+        * @param  p :Array pointer. counter :Number of array elements.
+    * @retval crc8
+    */
+    unsigned char CRC8_Table(unsigned char *p, int counter)  
+    {  
+        unsigned char crc8 = 0;  
+    
+        for( ; counter > 0; counter--)
+        {  
+        crc8 = CRC8Table[crc8^*p];  
+        p++;  
+        }  
+        return(crc8);  
+    }
+
+    RS485Driver::RS485Driver(size_t maxMotors) : max_motors_(maxMotors), serial_port_(-1), is_open_(false)
+    {
+
+    }
+    
+    RS485Driver::~RS485Driver()
+    {
+        closePort();
+    }
+    
+    speed_t RS485Driver::convertBaudRate(int baud_rate)
+    {
+        switch (baud_rate)
+        {
+            case 9600: return B9600;
+            case 19200: return B19200;
+            case 38400: return B38400;
+            case 57600: return B57600;
+            case 115200: return B115200;
+            case 230400: return B230400;
+            case 460800: return B460800;
+            case 500000: return B500000;
+            case 576000: return B576000;
+            case 921600: return B921600;
+            default: return B0;
+        }
+    }
+    
+    bool RS485Driver::openPort(const std::string &port_name, int baud_rate)
+    {
+        // 关闭已打开的端口
+        if (is_open_)
+        {
+            closePort();
+        }
+        
+        // 打开串口
+        serial_port_ = open(port_name.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
+        if (serial_port_ == -1)
+        {
+            std::cerr << "无法打开串口: " << port_name << std::endl;
+            return false;
+        }
+        
+        // 保存原始配置
+        if (tcgetattr(serial_port_, &original_termios_) == -1)
+        {
+            std::cerr << "无法获取串口属性" << std::endl;
+            close(serial_port_);
+            serial_port_ = -1;
+            return false;
+        }
+        
+        // 配置串口
+        struct termios tty;
+        memset(&tty, 0, sizeof(tty));
+        
+        // 设置波特率
+        speed_t speed = convertBaudRate(baud_rate);
+        if (speed == B0)
+        {
+            std::cerr << "不支持的波特率: " << baud_rate << std::endl;
+            closePort();
+            return false;
+        }
+        
+        cfsetospeed(&tty, speed);
+        cfsetispeed(&tty, speed);
+        
+        // 8位数据位，无奇偶校验，1位停止位
+        tty.c_cflag &= ~PARENB;  // 无奇偶校验
+        tty.c_cflag &= ~CSTOPB;  // 1位停止位
+        tty.c_cflag &= ~CSIZE;   // 清除数据位设置
+        tty.c_cflag |= CS8;      // 8位数据位
+        
+        // 禁用硬件流控制
+        tty.c_cflag &= ~CRTSCTS;
+        
+        // 启用接收器，设置本地模式
+        tty.c_cflag |= (CLOCAL | CREAD);
+        
+        // 禁用软件流控制
+        tty.c_iflag &= ~(IXON | IXOFF | IXANY);
+        
+        // 原始输入模式
+        tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+        
+        // 原始输出模式
+        tty.c_oflag &= ~OPOST;
+        
+        // 设置读取超时
+        tty.c_cc[VTIME] = 10;  // 1秒超时
+        tty.c_cc[VMIN] = 0;
+        
+        // 应用配置
+        if (tcsetattr(serial_port_, TCSANOW, &tty) != 0)
+        {
+            std::cerr << "无法设置串口属性" << std::endl;
+            closePort();
+            return false;
+        }
+        
+        is_open_ = true;
+        std::cout << "串口 " << port_name << " 已打开，波特率: " << baud_rate << std::endl;
+        return true;
+    }
+    
+    void RS485Driver::closePort()
+    {
+        if (is_open_)
+        {
+            // 恢复原始配置
+            tcsetattr(serial_port_, TCSANOW, &original_termios_);
+            close(serial_port_);
+            is_open_ = false;
+            serial_port_ = -1;
+            std::cout << "串口已关闭" << std::endl;
+        }
+    }
+    
+    bool RS485Driver::sendData(const std::vector<uint8_t> &data)
+    {
+        if (!is_open_)
+        {
+            std::cerr << "串口未打开，无法发送数据" << std::endl;
+            return false;
+        }
+        
+        ssize_t bytes_written = write(serial_port_, data.data(), data.size());
+        if (bytes_written != static_cast<ssize_t>(data.size()))
+        {
+            std::cerr << "发送数据失败，预期发送 " << data.size() 
+                      << " 字节，实际发送 " << bytes_written << " 字节" << std::endl;
+            return false;
+        }
+        
+        return true;
+    }
+    
+    bool RS485Driver::receiveData(std::vector<uint8_t> &data, size_t max_length, int timeout_ms)
+    {
+        if (!is_open_)
+        {
+            std::cerr << "串口未打开，无法接收数据" << std::endl;
+            return false;
+        }
+        
+        data.clear();
+        uint8_t buffer[256];
+        ssize_t bytes_read;
+        auto start_time = std::chrono::steady_clock::now();
+        
+        while (data.size() < max_length)
+        {
+            // 检查超时
+            auto current_time = std::chrono::steady_clock::now();
+            auto elapsed_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
+                current_time - start_time).count();
+            
+            if (elapsed_ms >= timeout_ms)
+            {
+                break;  // 超时
+            }
+            
+            // 尝试读取数据
+            bytes_read = read(serial_port_, buffer, std::min(sizeof(buffer), max_length - data.size()));
+            
+            if (bytes_read > 0)
+            {
+                data.insert(data.end(), buffer, buffer + bytes_read);
+                start_time = std::chrono::steady_clock::now();  // 重置超时计时器
+            }
+            else if (bytes_read < 0)
+            {
+                std::cerr << "接收数据错误" << std::endl;
+                return false;
+            }
+            else
+            {
+                // 没有数据，短暂休眠
+                std::this_thread::sleep_for(std::chrono::milliseconds(10));
+            }
+        }
+        
+        return !data.empty();
+    }
+
+    uint8_t RS485Driver::calculateChecksum(const std::vector<uint8_t> &data)
+    {
+        uint8_t checksum = 0;
+        for (uint8_t byte : data)
+        {
+            checksum ^= byte;
+        }
+        return checksum;
+    }
+    
+    bool RS485Driver::controlMotor(uint8_t motor_id, int32_t speed, bool stop)
+    {
+        if (!isOpen())
+        {
+            std::cerr << "RS485未初始化，无法控制电机" << std::endl;
+            return false;
+        }
+        
+        if (motor_id < 1 || motor_id > max_motors_)
+        {
+            std::cerr << "无效的电机ID: " << static_cast<int>(motor_id) 
+                      << ", 必须在1到" << static_cast<int>(max_motors_) << "之间" << std::endl;
+            return false;
+        }
+        
+        // 限制速度范围
+        if (speed > 1000) speed = 1000;
+        if (speed < -1000) speed = -1000;
+        
+        // 构建命令帧
+        std::vector<uint8_t> command;
+        command.push_back(START_BYTE);
+        
+        if (stop)
+        {
+            command.push_back(STOP_CMD);
+        }
+        else
+        {
+            command.push_back(CONTROL_CMD);
+        }
+        
+        command.push_back(motor_id);
+        
+        // 将速度转换为4字节并添加到命令
+        uint8_t speed_bytes[4];
+        std::memcpy(speed_bytes, &speed, sizeof(speed));
+        for (int i = 0; i < 4; ++i)
+        {
+            command.push_back(speed_bytes[i]);
+        }
+        
+        // 计算并添加校验和
+        uint8_t checksum = calculateChecksum(command);
+        command.push_back(checksum);
+        
+        // 添加结束字节
+        command.push_back(STOP_BYTE);
+        
+        // 发送命令
+        bool success = sendData(command);
+        
+        if (success)
+        {
+            if (stop)
+            {
+                std::cout << "已发送停止命令到电机 " << static_cast<int>(motor_id) << std::endl;
+            }
+            else
+            {
+                std::cout << "已发送命令到电机 " << static_cast<int>(motor_id) 
+                          << ", 速度: " << speed << std::endl;
+            }
+        }
+        else
+        {
+            std::cerr << "发送命令到电机 " << static_cast<int>(motor_id) << " 失败" << std::endl;
+        }
+        
+        return success;
+    }
+    
+    bool RS485Driver::controlAllMotors(const std::vector<int32_t> &speeds)
+    {
+        if (speeds.size() != max_motors_)
+        {
+            std::cerr << "控制所有电机需要提供 " << static_cast<int>(max_motors_) 
+                      << " 个速度值，实际提供了 " << speeds.size() << " 个" << std::endl;
+            return false;
+        }
+        
+        if (!isOpen())
+        {
+            std::cerr << "RS485未初始化，无法控制电机" << std::endl;
+            return false;
+        }
+        
+        // 构建命令帧
+        
+        bool success = false;
+
+        static uint8_t i = 1;
+
+        // 添加所有电机的速度
+
+        std::vector<uint8_t> command;
+
+        // 限制速度范围
+        int32_t clamped_speed = speeds[i - 1];
+        if (clamped_speed > 1000) clamped_speed = 1000;
+        if (clamped_speed < -1000) clamped_speed = -1000;
+
+        uint8_t high_byte = (clamped_speed >> 8) & 0xFF;  // 高8位
+        uint8_t low_byte  = clamped_speed & 0xFF;         // 低8位
+        
+        command.push_back(i);
+        command.push_back(CONTROL_CMD);
+        command.push_back(high_byte);
+        command.push_back(low_byte);
+        command.push_back(ZERO_CMD);
+        command.push_back(ZERO_CMD);
+        command.push_back(DEFAULT_TIME);
+        command.push_back(ZERO_CMD);
+        command.push_back(ZERO_CMD);
+
+        unsigned char crc8 = CRC8_Table(command.data(), 9);
+
+        command.push_back(crc8);
+        
+        // 发送命令
+        success = sendData(command);
+        std::cout << "已发送命令到电机 " << static_cast<int>(i) << ", 速度: " << clamped_speed << std::endl;
+        
+        ++i;
+
+        if (i > max_motors_)
+        {
+            i = 1;
+            return success;
+        }
+        
+        return false;
+    }
+    
+    bool RS485Driver::stopAllMotors()
+    {
+        std::vector<int32_t> speeds(max_motors_, 0);
+        return controlAllMotors(speeds);
+    }
+}
--- a/HiveCoreR0/src/robot_control/src/trapezoidal_trajectory.cpp
+++ b/HiveCoreR0/src/robot_control/src/trapezoidal_trajectory.cpp
@@ -0,0 +1,309 @@
+#include "trapezoidal_trajectory.hpp"
+#include <cmath>
+#include <stdexcept>
+#include <algorithm>
+#include <iostream>
+
+namespace robot_control
+{
+
+TrapezoidalTrajectory::TrapezoidalTrajectory(double max_velocity, double max_acceleration)
+    : max_velocity_(max_velocity), 
+      max_acceleration_(max_acceleration),
+      total_time_(0.0),
+      acceleration_time_(0.0),
+      deceleration_time_(0.0),
+      cruise_time_(0.0),
+      cruise_velocity_(0.0),
+      is_single_joint_(false)
+{
+    if (max_velocity <= 0)
+        throw std::invalid_argument("Max velocity must be positive");
+    if (max_acceleration <= 0)
+        throw std::invalid_argument("Max acceleration must be positive");
+}
+
+void TrapezoidalTrajectory::init(const std::vector<double>& start_pos, const std::vector<double>& target_pos, double duration)
+{
+    if (start_pos.size() != target_pos.size())
+        throw std::invalid_argument("Start and target position vectors must have the same size");
+    
+    is_single_joint_ = false;
+    start_pos_ = start_pos;
+    target_pos_ = target_pos;
+    current_pos_.resize(start_pos.size());
+    current_velocity_.resize(start_pos.size());
+    current_acceleration_.resize(start_pos.size());
+    is_stopping_ = false;
+    
+    calculateTrajectoryParams();
+    
+    // 如果指定了持续时间且大于最小时间，则重新计算参数
+    if (duration > 0 && duration > total_time_)
+    {
+        total_time_ = duration;
+        cruise_time_ = total_time_ - acceleration_time_ - deceleration_time_;
+    }
+}
+
+void TrapezoidalTrajectory::init(double start_pos, double target_pos, double duration)
+{
+    is_single_joint_ = true;
+    start_pos_ = {start_pos};
+    target_pos_ = {target_pos};
+    current_pos_ = {0.0};
+    current_velocity_ = {0.0};
+    current_acceleration_ = {0.0};
+    actual_deceleration_ = {0.0};
+    stop_time_ = 0.0;
+    
+    calculateTrajectoryParams();
+    
+    // 如果指定了持续时间且大于最小时间，则重新计算参数
+    if (duration > 0 && duration > total_time_)
+    {
+        total_time_ = duration;
+        cruise_time_ = total_time_ - acceleration_time_ - deceleration_time_;
+    }
+}
+
+void TrapezoidalTrajectory::calculateTrajectoryParams()
+{
+    // 计算总位移
+    double max_distance = 0.0;
+    for (size_t i = 0; i < start_pos_.size(); ++i)
+    {
+        max_distance = std::max(max_distance, std::fabs(target_pos_[i] - start_pos_[i]));
+    }
+    
+    if (max_distance < 1e-6)  // 位移过小，视为已到达
+    {
+        total_time_ = 0.0;
+        acceleration_time_ = 0.0;
+        deceleration_time_ = 0.0;
+        cruise_time_ = 0.0;
+        cruise_velocity_ = 0.0;
+        return;
+    }
+    
+    // 计算理论上的最大巡航速度和时间参数
+    double time_to_max_vel = max_velocity_ / max_acceleration_;
+    double distance_to_max_vel = 0.5 * max_acceleration_ * time_to_max_vel * time_to_max_vel;
+    
+    // 判断是否能达到最大速度
+    if (2 * distance_to_max_vel >= max_distance)
+    {
+        // 无法达到最大速度，没有匀速阶段
+        double t = std::sqrt(max_distance / max_acceleration_);
+        total_time_ = 2 * t;
+        acceleration_time_ = t;
+        deceleration_time_ = t;
+        cruise_time_ = 0.0;
+        cruise_velocity_ = max_acceleration_ * t;
+    }
+    else
+    {
+        // 可以达到最大速度，存在匀速阶段
+        acceleration_time_ = time_to_max_vel;
+        deceleration_time_ = time_to_max_vel;
+        cruise_time_ = (max_distance - 2 * distance_to_max_vel) / max_velocity_;
+        total_time_ = acceleration_time_ + cruise_time_ + deceleration_time_;
+        cruise_velocity_ = max_velocity_;
+    }
+
+    std::cout << "Total time: " << total_time_ << std::endl;
+    std::cout << "Acceleration time: " << acceleration_time_ << std::endl;
+    std::cout << "Deceleration time: " << deceleration_time_ << std::endl;
+    std::cout << "Cruise time: " << cruise_time_ << std::endl;
+}
+
+std::vector<double> TrapezoidalTrajectory::update(double time)
+{
+    if (is_single_joint_)
+        throw std::runtime_error("Calling update for multi-joint on a single-joint trajectory");
+    
+    // 限制时间在有效范围内
+    double t = std::clamp(time, 0.0, total_time_);
+    
+    // 计算当前阶段和加速度
+    double acceleration = 0.0;
+    double velocity = 0.0;
+    double position_factor = 0.0;
+    
+    if (t <= acceleration_time_)
+    {
+        // 加速阶段
+        acceleration = max_acceleration_;
+        velocity = acceleration * t;
+        position_factor = 0.5 * acceleration * t * t;
+    }
+    else if (t <= acceleration_time_ + cruise_time_)
+    {
+        // 匀速阶段
+        acceleration = max_acceleration_;
+        velocity = cruise_velocity_;
+        double t_cruise = t - acceleration_time_;
+        position_factor = 0.5 * acceleration * acceleration_time_ * acceleration_time_ + 
+                         velocity * t_cruise;
+    }
+    else
+    {
+        // 减速阶段
+        double t_decel = t - (acceleration_time_ + cruise_time_);
+        acceleration = -max_acceleration_;
+        velocity = cruise_velocity_ + acceleration * t_decel;
+        double distance_accel = 0.5 * max_acceleration_ * acceleration_time_ * acceleration_time_;
+        double distance_cruise = cruise_velocity_ * cruise_time_;
+        double distance_decel = cruise_velocity_ * t_decel + 0.5 * acceleration * t_decel * t_decel;
+        position_factor = distance_accel + distance_cruise + distance_decel;
+    }
+    
+    // 计算每个关节的位置、速度和加速度
+    for (size_t i = 0; i < start_pos_.size(); ++i)
+    {
+        double total_distance = target_pos_[i] - start_pos_[i];
+        double direction = total_distance > 0 ? 1.0 : -1.0;
+        double abs_distance = std::fabs(total_distance);
+        
+        current_pos_[i] = start_pos_[i] + direction * (abs_distance > 1e-6 ? 
+                          (position_factor * abs_distance / (0.5 * max_acceleration_ * acceleration_time_ * acceleration_time_ + 
+                          cruise_velocity_ * cruise_time_ + 
+                          cruise_velocity_ * deceleration_time_ - 0.5 * max_acceleration_ * deceleration_time_ * deceleration_time_)) : 0.0);
+        
+        current_velocity_[i] = direction * velocity;
+        current_acceleration_[i] = direction * acceleration;
+    }
+    
+    return current_pos_;
+}
+
+double TrapezoidalTrajectory::updateSingle(double time)
+{
+    if (!is_single_joint_)
+        throw std::runtime_error("Calling updateSingle for single-joint on a multi-joint trajectory");
+    
+    // 限制时间在有效范围内
+    double t = std::clamp(time, 0.0, total_time_);
+    
+    // 计算当前阶段和加速度
+    double acceleration = 0.0;
+    double velocity = 0.0;
+    double position_factor = 0.0;
+    
+    if (t <= acceleration_time_)
+    {
+        // 加速阶段
+        acceleration = max_acceleration_;
+        velocity = acceleration * t;
+        position_factor = 0.5 * acceleration * t * t;
+    }
+    else if (t <= acceleration_time_ + cruise_time_)
+    {
+        // 匀速阶段
+        acceleration = 0.0;
+        velocity = cruise_velocity_;
+        double t_cruise = t - acceleration_time_;
+        position_factor = 0.5 * acceleration * acceleration_time_ * acceleration_time_ + 
+                         velocity * t_cruise;
+    }
+    else
+    {
+        // 减速阶段
+        double t_decel = t - (acceleration_time_ + cruise_time_);
+        acceleration = -max_acceleration_;
+        velocity = cruise_velocity_ + acceleration * t_decel;
+        double distance_accel = 0.5 * max_acceleration_ * acceleration_time_ * acceleration_time_;
+        double distance_cruise = cruise_velocity_ * cruise_time_;
+        double distance_decel = cruise_velocity_ * t_decel + 0.5 * acceleration * t_decel * t_decel;
+        position_factor = distance_accel + distance_cruise + distance_decel;
+    }
+    
+    // 计算位置、速度和加速度
+    double total_distance = target_pos_[0] - start_pos_[0];
+    double direction = total_distance > 0 ? 1.0 : -1.0;
+    double abs_distance = std::fabs(total_distance);
+    
+    current_pos_[0] = start_pos_[0] + direction * (abs_distance > 1e-6 ? 
+                      (position_factor * abs_distance / (0.5 * max_acceleration_ * acceleration_time_ * acceleration_time_ + 
+                      cruise_velocity_ * cruise_time_ + 
+                      cruise_velocity_ * deceleration_time_ - 0.5 * max_acceleration_ * deceleration_time_ * deceleration_time_)) : 0.0);
+    
+    current_velocity_[0] = direction * velocity;
+    current_acceleration_[0] = direction * acceleration;
+    
+    return current_pos_[0];
+}
+
+void TrapezoidalTrajectory::setMaxVelocity(double max_velocity)
+{
+    if (max_velocity <= 0)
+        throw std::invalid_argument("Max velocity must be positive");
+    max_velocity_ = max_velocity;
+}
+
+void TrapezoidalTrajectory::setMaxAcceleration(double max_acceleration)
+{
+    if (max_acceleration <= 0)
+        throw std::invalid_argument("Max acceleration must be positive");
+    max_acceleration_ = max_acceleration;
+}
+
+inline double calculate_decel_time(double initial_velocity, double max_decel) {
+    if (std::fabs(initial_velocity) < 1e-9) { // 速度为0，无需减速
+        return 0.0;
+    }
+    return std::fabs(initial_velocity) / max_decel; // 时间 = 速度 / 减速度
+}
+
+void TrapezoidalTrajectory::StopCalculate()
+{
+    size_t dof = current_pos_.size(); // 自由度数量
+    std::vector<double> decel_times(dof);       // 各轴理论减速时间
+
+    for (size_t i = 0; i < dof; ++i) 
+    {
+        decel_times[i] = calculate_decel_time(current_velocity_[i], max_acceleration_);
+        if (decel_times[i] > stop_time_) {
+            stop_time_ = decel_times[i]; // 总时间取最长减速时间
+        }
+    }
+
+    // 特殊情况：所有轴已静止
+    if (stop_time_ < 1e-9) {
+        stop_time_ = 0.0;
+        return;
+    }
+
+    // 第二步：计算各轴实际减速度（确保在总时间内减速到0）
+    for (size_t i = 0; i < dof; ++i) {
+        if (std::fabs(current_velocity_[i]) < 1e-9) {
+            actual_deceleration_[i] = 0.0; // 静止轴减速度为0
+        } else {
+            // 实际减速度 = 初始速度 / 总时间（方向与速度相反）
+            actual_deceleration_[i] = -std::copysign(max_acceleration_, current_velocity_[i]) 
+                            * (decel_times[i] / stop_time_);
+        }
+    }
+}
+
+std::vector<double> TrapezoidalTrajectory::StopUpdate(double dt)
+{
+    for (size_t i = 0; i < current_pos_.size(); ++i) {
+        // 计算当前速度：v(t) = v0 + a*t
+        current_velocity_[i] = current_velocity_[i] + actual_deceleration_[i] * dt;
+        
+        // 计算当前位置：s(t) = s0 + v0*t + 0.5*a*t²
+        current_pos_[i] = current_pos_[i] 
+                        + current_velocity_[i] * dt 
+                        + 0.5 * actual_deceleration_[i] * dt * dt;
+
+        // 数值稳定性处理（避免因浮点误差出现微小负速度）
+        if (std::fabs(current_velocity_[i]) < 1e-9) {
+            current_velocity_[i] = 0.0;
+        }
+    }
+
+    return current_pos_;
+}
+
+}  // namespace robot_control
--- a/HiveCoreR0/src/robot_control/src/urdf_parser.cpp
+++ b/HiveCoreR0/src/robot_control/src/urdf_parser.cpp
@@ -5,8 +5,7 @@
 #include <stdexcept>
 #include <iostream>

-namespace robot_model
-{
+using namespace robot_control;

 std::shared_ptr<RobotModel> UrdfParser::parse_from_file(const std::string& urdf_file_path)
 {
@@ -246,4 +245,3 @@ std::string UrdfParser::identify_leg_side(const std::string& name)
    return position + side;
 }

-}  // namespace robot_model
--- a/HiveCoreR0/src/robot_control/src/wheel_control.cpp
+++ b/HiveCoreR0/src/robot_control/src/wheel_control.cpp
@@ -4,7 +4,7 @@ namespace robot_control
 {
    using namespace std::chrono_literals;

-    WheelControl::WheelControl(int totalJoints, double wheelRadius, double wheelSeparation, double maxLinearSpeed, double maxAngularSpeed)
+    WheelControl::WheelControl(int totalJoints, double wheelRadius, double wheelSeparation, double maxLinearSpeed, double maxAngularSpeed, std::vector<int32_t> wheelDirection)
    {
      
        totalJoints_ = totalJoints;  // 总关节数
@@ -12,9 +12,28 @@ namespace robot_control
        wheelSeparation_ = wheelSeparation;  // 轮子间距
        maxLinearSpeed_ = maxLinearSpeed;  // 线速度
        maxAngularSpeed_ = maxAngularSpeed;  // 角速度
+        wheelDirection_ = wheelDirection;  // 轮子方向
  
        // 初始化轮子目标角度
-        wheelTargets_.resize(totalJoints_, 0.0);
+        wheelSpeed_.resize(totalJoints_, 0);
+        previousWheelSpeed_.resize(totalJoints_, 0);
+
+        isSendFinished_ = true;
+
+        // 初始化串口通信
+        if (!rs485_driver_.isOpen())
+        {
+            std::cout << "open port!" << std::endl;
+
+            if (rs485_driver_.openPort("/dev/ttyS0", 115200)) // 打开串口
+            {
+                std::cout << "Serial port opened successfully." << std::endl;
+            }
+            else
+            {
+                std::cout << "Failed to open serial port." << std::endl;
+            }
+        }
    }

    WheelControl::~WheelControl()
@@ -26,31 +45,25 @@ namespace robot_control
    // 关节复位
    void WheelControl::ResetJoints()
    {
-        wheelTargets_.resize(totalJoints_, 0.0);
+        wheelSpeed_.resize(totalJoints_, 0);
    }

-    void WheelControl::Move(std::vector<double>& deltaJointPositions, double dt)
-    {
-        CalculateWheelTargets(deltaJointPositions, dt);
-    }

-    void WheelControl::CalculateWheelTargets(std::vector<double>& deltaJointPositions, double dt_sec)
+    void WheelControl::CalculateWheelSpeeds()
    {
        // 计算左右轮目标线速度
        double leftSpeed = linearSpeed_ + (angularSpeed_ * wheelSeparation_) / 2.0;
        double rightSpeed = linearSpeed_ - (angularSpeed_ * wheelSeparation_) / 2.0;

        // 计算轮子角速度（rad/s）
-        double leftOmega = leftSpeed / wheelRadius_;
-        double rightOmega = rightSpeed / wheelRadius_;
+        int32_t leftOmega = leftSpeed ;
+        int32_t rightOmega = rightSpeed;

-        // 积分计算目标角度（弧度）
-        wheelTargets_[0] = leftOmega * dt_sec;  // left_wheel1
-        wheelTargets_[1] = rightOmega * dt_sec; // right_wheel1
-        wheelTargets_[2] = leftOmega * dt_sec;  // left_wheel2
-        wheelTargets_[3] = rightOmega * dt_sec; // right_wheel2
+        wheelSpeed_[0] = wheelDirection_[0] * leftOmega;  // left_wheel1
+        wheelSpeed_[1] = wheelDirection_[1] * rightOmega; // right_wheel1
+        wheelSpeed_[2] = wheelDirection_[2] * leftOmega;  // left_wheel2
+        wheelSpeed_[3] = wheelDirection_[3] * rightOmega; // right_wheel2

-        deltaJointPositions = wheelTargets_;
    }

    void WheelControl::setLinearAngularSpeed(double linearSpeed, double angularSpeed)
@@ -80,4 +93,19 @@ namespace robot_control
            angularSpeed_ = angularSpeed;
        }
    }
+
+    void WheelControl::ExecuteCommand()
+    {
+        CalculateWheelSpeeds();
+
+        if (wheelSpeed_ != previousWheelSpeed_)
+        {
+            isSendFinished_ = rs485_driver_.controlAllMotors(wheelSpeed_);
+            if (isSendFinished_)
+            {
+                previousWheelSpeed_ = wheelSpeed_;
+            }
+        }
+        
+    }
 }