diff --git a/HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver/src/ethercat_driver.cpp b/HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver/src/ethercat_driver.cpp index 5365d69..13a23ab 100644 --- a/HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver/src/ethercat_driver.cpp +++ b/HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver/src/ethercat_driver.cpp @@ -302,8 +302,6 @@ CallbackReturn EthercatDriver::on_activate( } // start EC and wait until state operative - control_frequency_=100.0f; - std::cout<<"force set freq"< STATE_FAULT (automatic) return control_word; case STATE_FAULT: // -> STATE_SWITCH_ON_DISABLED + // std::cout << "EcCiA402Drive: Fault detected. Resetting drive." << std::endl; if (auto_fault_reset_ || fault_reset_) { + // if (false || fault_reset_) { fault_reset_ = false; return (control_word & 0b11111111) | 0b10000000; // automatic reset } else { diff --git a/HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/include/ethercat_interface/ec_master.hpp b/HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/include/ethercat_interface/ec_master.hpp index 0bee253..ff450f1 100644 --- a/HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/include/ethercat_interface/ec_master.hpp +++ b/HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/include/ethercat_interface/ec_master.hpp @@ -83,9 +83,7 @@ public: void setCtrlFrequency(double frequency) { - std::cout << "set control frequency to " << frequency << std::endl; interval_ = 1000000000.0 / frequency; - std::cout << "new interval_ " << interval_<< std::endl; } uint32_t getInterval() {return interval_;} diff --git 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 index 18531f8..b46fd3b 100644 --- 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,6 +1,6 @@ controller_manager: ros__parameters: - update_rate: 100 # Hz + update_rate: 1000 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster @@ -20,6 +20,16 @@ controller_manager: # - position # joints: # - joint_1 +# - joint_2 +# - joint_3 +# - joint_4 +# - joint_5 +# - joint_6 +# - joint_7 +# - joint_8 +# - joint_9 +# - joint_10 +# - joint_11 gpio_command_controller: ros__parameters: diff --git 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 index 7a47a38..9a478c5 100644 --- 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 @@ -2,7 +2,7 @@ vendor_id: 0x5a65726f product_id: 0x00029252 assign-activate: 0x0300 #DC synch register -auto_fault_reset: true +auto_fault_reset: false auto_enable_set: false # sdo: # sdo data to be transferred at drive startup diff --git 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 index c8ed852..cf04d45 100644 --- 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 @@ -3,7 +3,7 @@ - + diff --git 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 index fde7c2d..c751639 100644 --- 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 @@ -89,11 +89,11 @@ def generate_launch_description(): arguments=["gpio_command_controller", "-c", "/controller_manager"], ) - # trajectory_controller_spawner = Node( - # package="controller_manager", - # executable="spawner", - # arguments=["trajectory_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", @@ -111,6 +111,7 @@ def generate_launch_description(): control_node, robot_state_pub_node, joint_state_broadcaster_spawner, + # trajectory_controller_spawner, gpio_controller_spawner, ] diff --git a/HiveCoreR0/src/joy_control/joy_control/joy_control_node.py b/HiveCoreR0/src/joy_control/joy_control/joy_control_node.py index 18bc9b2..c9d9721 100644 --- a/HiveCoreR0/src/joy_control/joy_control/joy_control_node.py +++ b/HiveCoreR0/src/joy_control/joy_control/joy_control_node.py @@ -59,9 +59,21 @@ class JoyControlNode(Node): if msg.buttons[0] == 1: command = 'A' self.command_publisher.publish(String(data=command)) - if msg.buttons[1] == 1: + elif msg.buttons[1] == 1: command = 'B' self.command_publisher.publish(String(data=command)) + elif msg.buttons[2] == 1: + command = 'X' + self.command_publisher.publish(String(data=command)) + elif msg.buttons[3] == 1: + command = 'Y' + self.command_publisher.publish(String(data=command)) + elif msg.buttons[4] == 1: + command = 'LB' + self.command_publisher.publish(String(data=command)) + elif msg.buttons[5] == 1: + command = 'RB' + self.command_publisher.publish(String(data=command)) # 处理摇杆（死区0.1） deadzone = 0.1 @@ -80,6 +92,14 @@ class JoyControlNode(Node): if msg.axes[1] != self.previous_axes[1] and msg.axes[1] < - deadzone : command = '后退' self.command_publisher.publish(String(data=command)) + + if msg.axes[6] != self.previous_axes[6] and msg.axes[6] > deadzone : + command = '抬手' + self.command_publisher.publish(String(data=command)) + + if msg.axes[6] != self.previous_axes[6] and msg.axes[6] < - deadzone : + command = '放手' + self.command_publisher.publish(String(data=command)) if msg.axes[7] != self.previous_axes[7] and msg.axes[7] > deadzone : command = '上升' @@ -89,8 +109,8 @@ class JoyControlNode(Node): command = '下降' self.command_publisher.publish(String(data=command)) - if msg.axes[0] != self.previous_axes[0] or msg.axes[1] != self.previous_axes[1] or msg.axes[7] != self.previous_axes[7]: - if msg.axes[0] == 0.0 and msg.axes[1] == 0.0 and msg.axes[7] == 0.0: + if msg.axes[0] != self.previous_axes[0] or msg.axes[1] != self.previous_axes[1] or msg.axes[7] != self.previous_axes[7] or msg.axes[6] != self.previous_axes[6]: + if msg.axes[0] == 0.0 and msg.axes[1] == 0.0 and msg.axes[7] == 0.0 and msg.axes[6] == 0.0: command = '停止' self.command_publisher.publish(String(data=command)) @@ -109,4 +129,4 @@ def main(args=None): if __name__ == '__main__': main() - \ No newline at end of file + diff --git a/HiveCoreR0/src/robot_control/include/arm_control.hpp b/HiveCoreR0/src/robot_control/include/arm_control.hpp index 873c417..f42ade6 100644 --- a/HiveCoreR0/src/robot_control/include/arm_control.hpp +++ b/HiveCoreR0/src/robot_control/include/arm_control.hpp @@ -31,6 +31,10 @@ namespace robot_control bool Stop(std::vector& joint_positions, double dt); + bool MoveUp(std::vector& joint_positions, double dt); + + bool MoveDown(std::vector& joint_positions, double dt); + void UpdateJointStates(const std::vector& joint_positions, const std::vector& joint_velocities, const std::vector& joint_torques); bool IsMoving(); diff --git a/HiveCoreR0/src/robot_control/include/common_enum.hpp b/HiveCoreR0/src/robot_control/include/common_enum.hpp index 3cff53f..65d80ac 100644 --- a/HiveCoreR0/src/robot_control/include/common_enum.hpp +++ b/HiveCoreR0/src/robot_control/include/common_enum.hpp @@ -16,6 +16,8 @@ namespace robot_control TURNING_RIGHT, // 右转 MOVING_UP, // 上升（腰部） MOVING_DOWN, // 下降（腰部） + ARM_MOVING_UP, // 手臂上升 + ARM_MOVING_DOWN, // 手臂下降 HOME_MOVING // 回零 }; @@ -30,6 +32,8 @@ namespace robot_control RIGHT, // 右转 UP, // 上升 DOWN, // 下降 + HAND_UP, // 手臂上升 + HAND_DOWN, // 手臂下降 STOP, // 停止 ARM_GRAB, // 手臂抓取 GO_HOME, // 关节回零 diff --git a/HiveCoreR0/src/robot_control/include/motion_parameters.hpp b/HiveCoreR0/src/robot_control/include/motion_parameters.hpp index fd57ae3..ee3423d 100644 --- a/HiveCoreR0/src/robot_control/include/motion_parameters.hpp +++ b/HiveCoreR0/src/robot_control/include/motion_parameters.hpp @@ -45,26 +45,26 @@ namespace robot_control arm_length_l2_ = 0.219; // 轮子速度参数 - max_linear_speed_x_ = 10; + max_linear_speed_x_ = 100; max_linear_speed_z_ = 10; max_angular_speed_z_ = 50; // 关节速度参数 - max_joint_velocity_ = 10; // 10 度/秒 - max_joint_acceleration_ = 100; // 10 度/秒^2 + max_joint_velocity_ = 5; // 10 度/秒 + max_joint_acceleration_ = 25; // 10 度/秒^2 // 初始化关节索引 - wheel_joint_indices_ = {0, 1, 2, 3}; - pitch_body_indices_ = {0}; - right_leg_joint_indices_ = {1}; - left_leg_joint_indices_ = {2}; - right_arm_joint_indices_ = {3, 4, 5, 6}; - left_arm_joint_indices_ = {7, 8, 9, 10}; + wheel_joint_indices_ = {0, 1}; + pitch_body_indices_ = {8}; + right_leg_joint_indices_ = {9}; + left_leg_joint_indices_ = {10}; + right_arm_joint_indices_ = {0, 1, 2, 3}; + left_arm_joint_indices_ = {4, 5, 6, 7}; + + total_joints_ = 11 ; //left_arm_joint_indices_.size() + right_arm_joint_indices_.size() + pitch_body_indices_.size() + left_leg_joint_indices_.size() + right_leg_joint_indices_.size(); - total_joints_ = 11 ; //left_arm_joint_indices_.size() + right_arm_joint_indices_.size() + pitch_body_indices_.size() + left_leg_joint_indices_.size() + right_leg_joint_indices_.size(); - // 初始化关节方向 - wheel_joint_directions_ = {1, -1, 1, -1}; + wheel_joint_directions_ = {-1, 1}; left_arm_joint_directions_ = {1, 1, 1, 1}; right_arm_joint_directions_ = {1, 1, 1, 1}; pitch_body_directions_ = {1}; @@ -152,6 +152,8 @@ namespace robot_control pulse_to_radian_ = 2.0 * M_PI / 524288.0; radian_to_pulse_ = 524288.0 / (2.0 * M_PI); + jog_step_size_ = 10 * pulse_to_degree_; + serial_port_ = "/dev/ttyUSB0"; baud_rate_ = 115200; @@ -210,6 +212,7 @@ namespace robot_control double max_joint_velocity_; double max_joint_acceleration_; + double jog_step_size_; // 运动状态 std::vector joint_positions_; // 关节位置（弧度） diff --git a/HiveCoreR0/src/robot_control/include/robot_control_manager.hpp b/HiveCoreR0/src/robot_control/include/robot_control_manager.hpp index 539d185..17d10d0 100644 --- a/HiveCoreR0/src/robot_control/include/robot_control_manager.hpp +++ b/HiveCoreR0/src/robot_control/include/robot_control_manager.hpp @@ -26,12 +26,19 @@ namespace robot_control void TurnRight(); void MoveUp(double dt); void MoveDown(double dt); + void ArmMoveUp(double dt); + void ArmMoveDown(double dt); bool Stop(double dt); bool GoHome(double dt); + void JogAxis(int axis, int dir); // void ArmGrab(double dt); + void UpdateInitCommand(); + // 机器人关节指令 std_msgs::msg::Float64MultiArray GetJointCommands(); + std_msgs::msg::Float64MultiArray GetJointInitCommands(); + void CheckJointLimits(std_msgs::msg::Float64MultiArray& jointCommands); void UpdateJointStates(const sensor_msgs::msg::JointState::SharedPtr msg); @@ -66,6 +73,10 @@ namespace robot_control sensor_msgs::msg::JointState jointStates_; std_msgs::msg::Float64MultiArray jointCommands_; + std_msgs::msg::Float64MultiArray jointCommandsInit_; + + void AssignTempValues(); + void UpdateJointCommands(); }; diff --git a/HiveCoreR0/src/robot_control/include/robot_fsm.hpp b/HiveCoreR0/src/robot_control/include/robot_fsm.hpp index 32ae4a6..48e5870 100644 --- a/HiveCoreR0/src/robot_control/include/robot_fsm.hpp +++ b/HiveCoreR0/src/robot_control/include/robot_fsm.hpp @@ -36,7 +36,9 @@ namespace robot_control // 状态转换为字符串 std::string StateToString(RobotState state) const; - void EnableRobot(); + void EnableRobot(double value); + void ResetFault(double value); + int motor_dst(std::string name,double dst);//add by hehe private: rclcpp::Publisher::SharedPtr cmdPub_; @@ -62,10 +64,20 @@ namespace robot_control bool isPaused_; bool isFinished_; bool isRobotEnabled_; + bool isFaultReset_; + bool isJogMode_; bool enableCommandExecuted_; + // bool resetFault_; int jogDirection_; - + int jogIndex_; + double jogValue_; + //add by hehe + std::unordered_map curMap_; + std::unordered_map dstMap_; + control_msgs::msg::DynamicInterfaceGroupValues posMsg_; + sensor_msgs::msg::JointState curJointSta_; + //add by hehe end // 状态转换表 std::map, RobotState> transitionTable_; @@ -88,4 +100,4 @@ namespace robot_control } // namespace robot_control -#endif // ROBOT_FSM_H \ No newline at end of file +#endif // ROBOT_FSM_H diff --git a/HiveCoreR0/src/robot_control/include/rs485_driver.hpp b/HiveCoreR0/src/robot_control/include/rs485_driver.hpp index 9174d09..65ae2f8 100644 --- a/HiveCoreR0/src/robot_control/include/rs485_driver.hpp +++ b/HiveCoreR0/src/robot_control/include/rs485_driver.hpp @@ -10,7 +10,7 @@ namespace robot_control class RS485Driver { public: - RS485Driver(size_t max_motors = 4); + RS485Driver(size_t max_motors = 2); ~RS485Driver(); // 打开串口 @@ -45,6 +45,8 @@ namespace robot_control bool is_open_; struct termios original_termios_; size_t max_motors_; + + size_t control_index_; // 转换波特率 speed_t convertBaudRate(int baud_rate); diff --git a/HiveCoreR0/src/robot_control/include/wheel_control.hpp b/HiveCoreR0/src/robot_control/include/wheel_control.hpp index 9b4577e..df0bd41 100644 --- a/HiveCoreR0/src/robot_control/include/wheel_control.hpp +++ b/HiveCoreR0/src/robot_control/include/wheel_control.hpp @@ -20,7 +20,9 @@ namespace robot_control void setLinearAngularSpeed(double linearSpeed, double angularSpeed); - void ExecuteCommand(); + bool ExecuteCommand(); + + double GetLinearSpeed() {return linearSpeed_;} private: diff --git a/HiveCoreR0/src/robot_control/src/arm_control.cpp b/HiveCoreR0/src/robot_control/src/arm_control.cpp index dc697df..886da6d 100644 --- a/HiveCoreR0/src/robot_control/src/arm_control.cpp +++ b/HiveCoreR0/src/robot_control/src/arm_control.cpp @@ -121,6 +121,46 @@ bool ArmControl::MoveToTargetJoint(std::vector& joint_positions, const s return false; } +bool ArmControl::MoveUp(std::vector& joint_positions, double dt) +{ + if (!is_moving_) + { + for (size_t i = 0; i < joint_position_.size(); i++) + { + if (joint_directions_[i] == 1) + { + joint_position_desired_[i] = joint_position_[i] + 20; + } + else + { + joint_position_desired_[i] = joint_position_[i] - 20; + } + } + } + + return MoveToTargetJoint(joint_positions, joint_position_desired_, dt); +} + +bool ArmControl::MoveDown(std::vector& joint_positions, double dt) +{ + if (!is_moving_) + { + for (size_t i = 0; i < joint_position_.size(); i++) + { + if (joint_directions_[i] == 1) + { + joint_position_desired_[i] = joint_position_[i] - 20; + } + else + { + joint_position_desired_[i] = joint_position_[i] + 20; + } + } + } + + return MoveToTargetJoint(joint_positions, joint_position_desired_, dt); +} + bool ArmControl::GoHome(std::vector& joint_positions, double dt) { bool all_joints_at_home = true; // 标记是否所有关节都在零点 @@ -195,9 +235,7 @@ bool ArmControl::Stop(std::vector& joint_positions, double dt) if (is_moving_ && !is_stopping_) { is_stopping_ = true; - std::cout << "arm control stop1" << std::endl; trapezoidalTrajectory_->StopCalculate(); - std::cout << "arm control stop2" << std::endl; stopDurationTime_ = 0.0; } diff --git a/HiveCoreR0/src/robot_control/src/leg_control.cpp b/HiveCoreR0/src/robot_control/src/leg_control.cpp index da34152..857efb9 100644 --- a/HiveCoreR0/src/robot_control/src/leg_control.cpp +++ b/HiveCoreR0/src/robot_control/src/leg_control.cpp @@ -108,11 +108,11 @@ bool LegControl::MoveUp(std::vector& joint_positions, double dt) { if (joint_directions_[i] == 1) { - joint_position_desired_[i] = maxLimits_[i]; + joint_position_desired_[i] = joint_position_[i] + 20; } else { - joint_position_desired_[i] = minLimits_[i]; + joint_position_desired_[i] = joint_position_[i] - 20; } } } @@ -128,11 +128,11 @@ bool LegControl::MoveDown(std::vector& joint_positions, double dt) { if (joint_directions_[i] == 1) { - joint_position_desired_[i] = minLimits_[i]; + joint_position_desired_[i] = joint_position_[i] - 20; } else { - joint_position_desired_[i] = maxLimits_[i]; + joint_position_desired_[i] = joint_position_[i] + 20; } } } diff --git a/HiveCoreR0/src/robot_control/src/robot_control_manager.cpp b/HiveCoreR0/src/robot_control/src/robot_control_manager.cpp index d03d088..74071ae 100644 --- a/HiveCoreR0/src/robot_control/src/robot_control_manager.cpp +++ b/HiveCoreR0/src/robot_control/src/robot_control_manager.cpp @@ -21,6 +21,7 @@ void RobotControlManager::init() for (size_t i = 0; i < motionParams_.total_joints_; i++) { jointCommands_.data.push_back(0.0); + jointCommandsInit_.data.push_back(0.0); } leftArmController_ = new ArmControl( @@ -112,6 +113,31 @@ Float64MultiArray RobotControlManager::GetJointCommands() return tempValues; } +Float64MultiArray RobotControlManager::GetJointInitCommands() +{ + Float64MultiArray tempValues; + tempValues.data.resize(motionParams_.total_joints_); + + // Convert the joint commands to motor values and limit to encoder range + for (size_t i = 0; i < jointCommandsInit_.data.size(); i++) + { + double angle = jointCommandsInit_.data[i]; + + double normalizedAngle = fmod(angle, 360.0); + + // 计算原始脉冲值 + double pulseValue = normalizedAngle * motionParams_.degree_to_pulse_; + + tempValues.data[i] = pulseValue; + } + + return tempValues; +} + +void RobotControlManager::UpdateInitCommand() +{ + jointCommandsInit_ = jointCommands_; +} void RobotControlManager::CheckJointLimits(Float64MultiArray &tempJointValues) { @@ -139,8 +165,7 @@ bool isAllTrueManual(const std::vector& vec) { return true; // 所有元素都是 true } -bool isPrint = false; - +bool isValueSet = false; void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::SharedPtr msg) { jointStates_ = *msg; @@ -150,36 +175,66 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState:: for (size_t i = 0; i < motionParams_.total_joints_; i++) { - jointPositions_[i] = jointStates_.position[i] * motionParams_.pulse_to_degree_; // convert to degree - jointVelocities_[i] = jointStates_.velocity[i] * motionParams_.pulse_to_degree_; - jointEfforts_[i] = 0.0; //jointStates_.effort[i] ; // TODO: check the factory + std::string joint_name = jointStates_.name[i]; + int index = -1; + size_t underscore_pos = joint_name.find_last_of('_'); + if (underscore_pos != std::string::npos && underscore_pos < joint_name.size() - 1) + { + std::string num_str = joint_name.substr(underscore_pos + 1); + try + { + index = std::stoi(num_str) - 1; + } + catch (const std::invalid_argument& e) + { + std::cerr << "关节名称 " << joint_name << " 格式错误：下划线后非数字" << std::endl; + } + } + + if (index != -1 && index < motionParams_.total_joints_) + { + jointPositions_[index] = jointStates_.position[i] * motionParams_.pulse_to_degree_; + jointVelocities_[index] = jointStates_.velocity[i] * motionParams_.pulse_to_degree_; + jointEfforts_[index] = 0.0; + } + + // else do not update. } + // TODO: This block can be deleted for (size_t i = 0; i < motionParams_.total_joints_; i++) { if(!jointInited_[i]) { - jointCommands_.data[i] = jointStates_.position[i]* motionParams_.pulse_to_degree_; - - if(jointCommands_.data[i] != 0) + if(jointPositions_[i] != 0) { jointInited_[i] = true; } } } - if (isAllTrueManual(jointInited_) && !isPrint) + static int count = 0; + if (isAllTrueManual(jointInited_) && !isValueSet) { - isPrint = true; - std::cout << "All joints are initialized" << std::endl; - for (size_t i = 0; i < motionParams_.total_joints_; i++) + if (count > 5000) { - std::cout << "Joint positions: " << i + 1 << " " << jointCommands_.data[i] << std::endl; + jointCommands_.data = jointPositions_; + jointCommandsInit_.data = jointPositions_; + isValueSet = true; + count = 0; + std::cout << "Joint commands set to joint positions" << std::endl; + std::cout << "All joints are initialized" << std::endl; + for (size_t i = 0; i < motionParams_.total_joints_; i++) + { + std::cout << "Joint positions: " << i + 1 << " " << jointCommands_.data[i] << std::endl; + } } + count++; } + // Update the arm controller size_t armJointSize = motionParams_.left_arm_joint_indices_.size(); std::vector armPositions(jointPositions_.begin() + motionParams_.left_arm_joint_indices_[0], jointPositions_.begin() + motionParams_.left_arm_joint_indices_[0] + armJointSize); @@ -225,85 +280,96 @@ void RobotControlManager::MoveBackward() void RobotControlManager::TurnLeft() { - wheelController_ -> setLinearAngularSpeed(0.0, - motionParams_.max_angular_speed_z_); + wheelController_ -> setLinearAngularSpeed(0.0, motionParams_.max_angular_speed_z_); wheelController_ -> ExecuteCommand(); } void RobotControlManager::TurnRight() { - wheelController_ -> setLinearAngularSpeed(0.0, motionParams_.max_angular_speed_z_); + wheelController_ -> setLinearAngularSpeed(0.0, - motionParams_.max_angular_speed_z_); wheelController_ -> ExecuteCommand(); } +bool executeFinished = true; bool RobotControlManager::Stop(double dt) { - wheelController_ -> setLinearAngularSpeed(0.0, 0.0); - wheelController_ -> ExecuteCommand(); + double currentSpeed = wheelController_ -> GetLinearSpeed(); + bool wheelStopped = abs(currentSpeed) > 0 ? false : true; + + if(executeFinished) + { + double tempSpeed = 0.0; + if(currentSpeed < -1.0) + { + tempSpeed = currentSpeed + 1.0; + } + else if(currentSpeed > 5.0) + { + tempSpeed = currentSpeed - 1.0; + } + + wheelController_ -> setLinearAngularSpeed(tempSpeed, 0.0); + // std::cout << "set speed : " << tempSpeed << std::endl; + } + + executeFinished = wheelController_ -> ExecuteCommand(); + + AssignTempValues(); bool leftLegStopped = leftLegController_->Stop(tempLeftLegCmd, dt); - bool rightLegStopped = rightLegController_->Stop(tempRightLegCmd, dt); - bool leftArmStopped = leftArmController_->Stop(tempLeftArmCmd, dt); - bool rightArmStopped = rightArmController_->Stop(tempRightArmCmd, dt); + + UpdateJointCommands(); - for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++) - { - jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = tempLeftLegCmd[i]; - } - for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++) - { - jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = tempRightLegCmd[i]; - } - - //TODO: there is no data from arm - // 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]; - // } - - return leftLegStopped && rightLegStopped && leftArmStopped && rightArmStopped; + return leftLegStopped && rightLegStopped && leftArmStopped && rightArmStopped && wheelStopped && executeFinished; } void RobotControlManager::MoveUp(double dt) { + AssignTempValues(); + leftLegController_ -> MoveUp(tempLeftLegCmd, dt); rightLegController_ -> MoveUp(tempRightLegCmd, dt); wheelController_-> setLinearAngularSpeed(5.0, 0.0); //TODO: value need to be changed wheelController_ -> ExecuteCommand(); - for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++) - { - jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = tempLeftLegCmd[i]; - } - for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++) - { - jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = tempRightLegCmd[i]; - } + UpdateJointCommands(); } void RobotControlManager::MoveDown(double dt) { + AssignTempValues(); + leftLegController_ -> MoveDown(tempLeftLegCmd, dt); rightLegController_ -> MoveDown(tempRightLegCmd, dt); wheelController_-> setLinearAngularSpeed(-5.0, 0.0); //TODO: value need to be changed wheelController_ -> ExecuteCommand(); - for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++) - { - jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = tempLeftLegCmd[i]; - } - for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++) - { - jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = tempRightLegCmd[i]; - } + UpdateJointCommands(); +} + +void RobotControlManager::ArmMoveUp(double dt) +{ + AssignTempValues(); + + leftArmController_ -> MoveUp(tempLeftArmCmd, dt); + rightArmController_ -> MoveUp(tempRightArmCmd, dt); + + UpdateJointCommands(); +} + +void RobotControlManager::ArmMoveDown(double dt) +{ + AssignTempValues(); + + leftArmController_ -> MoveDown(tempLeftArmCmd, dt); + rightArmController_ -> MoveDown(tempRightArmCmd, dt); + + UpdateJointCommands(); } bool RobotControlManager::GoHome(double dt) @@ -346,3 +412,59 @@ bool RobotControlManager::GoHome(double dt) return robotHomed_[0] && robotHomed_[1];// && homeFinished[2] && homeFinished[3]; } + +void RobotControlManager::JogAxis(int axis, int direction) +{ + if(axis < 0 || axis > motionParams_.total_joints_) + { + return; + } + + jointCommands_.data[axis] += direction * motionParams_.jog_step_size_; +} + +void RobotControlManager::AssignTempValues() +{ + tempLeftArmCmd.resize(motionParams_.left_arm_joint_indices_.size()); + tempRightArmCmd.resize(motionParams_.right_arm_joint_indices_.size()); + tempLeftLegCmd.resize(motionParams_.left_leg_joint_indices_.size()); + tempRightLegCmd.resize(motionParams_.right_leg_joint_indices_.size()); + + for (size_t i = 0; i < motionParams_.left_arm_joint_indices_.size(); i++) + { + tempLeftArmCmd[i] = jointCommands_.data[motionParams_.left_arm_joint_indices_[i]]; + } + for (size_t i = 0; i < motionParams_.right_arm_joint_indices_.size(); i++) + { + tempRightArmCmd[i] = jointCommands_.data[motionParams_.right_arm_joint_indices_[i]]; + } + for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++) + { + tempLeftLegCmd[i] = jointCommands_.data[motionParams_.left_leg_joint_indices_[i]]; + } + for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++) + { + tempRightLegCmd[i] = jointCommands_.data[motionParams_.right_leg_joint_indices_[i]]; + } +} + +void RobotControlManager::UpdateJointCommands() +{ + // TODO: first joint will conflict with righ arm, so we start from the second one + for (size_t i = 1; i < motionParams_.left_arm_joint_indices_.size(); i++) + { + jointCommands_.data[motionParams_.left_arm_joint_indices_[i]] = tempLeftArmCmd[i]; + } + for (size_t i = 1; i < motionParams_.right_arm_joint_indices_.size(); i++) + { + jointCommands_.data[motionParams_.right_arm_joint_indices_[i]] = tempRightArmCmd[i]; + } + for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++) + { + jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = tempLeftLegCmd[i]; + } + for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++) + { + jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = tempRightLegCmd[i]; + } +} diff --git a/HiveCoreR0/src/robot_control/src/robot_fsm.cpp b/HiveCoreR0/src/robot_control/src/robot_fsm.cpp index 3ce03a0..555147a 100644 --- a/HiveCoreR0/src/robot_control/src/robot_fsm.cpp +++ b/HiveCoreR0/src/robot_control/src/robot_fsm.cpp @@ -14,9 +14,12 @@ RobotFsm::RobotFsm() : Node("robot_Fsm_node") previousState_ = RobotState::INIT; isRobotEnabled_ = false; + isFaultReset_ = false; + isJogMode_ = false; enableCommandExecuted_ = false; jogDirection_ = 0; - + jogIndex_ = 5; //default axis 6 + // 初始化数据文件（设置路径，确保目录存在） data_file_path_ = "/home/demo/ros2_joint_data.txt"; // 数据保存路径 fs::path file_path(data_file_path_); @@ -57,8 +60,9 @@ RobotFsm::RobotFsm() : Node("robot_Fsm_node") lastTime_ = this->now(); // 初始化时间 // 创建定时器，每10ms执行一次控制逻辑（频率100Hz） - controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(10),std::bind(&RobotFsm::ControlLoop, this)); // 绑定到新的控制函数); - + controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(1),std::bind(&RobotFsm::ControlLoop, this)); // 绑定到新的控制函数); + curMap_.clear(); + dstMap_.clear(); std::cout << "RobotFsm Node is created finished!" << std::endl; } @@ -70,14 +74,34 @@ RobotFsm::~RobotFsm() std::cout << "Robot controller stopped." << std::endl; } +void PrintMessage(control_msgs::msg::DynamicInterfaceGroupValues msg) { + std::cout << "publish joint trajectory" << std::endl; + std::cout << msg.header.stamp.sec << std::endl; + std::cout << msg.header.stamp.nanosec << std::endl; + std::cout << msg.header.frame_id << std::endl; + + for (size_t i = 0; i < msg.interface_groups.size(); i++) + { + std::cout << msg.interface_groups[i] << std::endl; + } + + for (size_t i = 0; i < msg.interface_values.size(); i++) + { + std::cout << msg.interface_values[i].interface_names[0]; + std::cout << msg.interface_values[i].values[0] << std::endl; + } +} + void RobotFsm::JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg) { if (!msg) { // 检查消息是否有效 std::cout << "get null joint states!" << std::endl; return; } - - // 更新机器人状态 + for(size_t i=0; i < msg-> name.size();i++) + { + curMap_[msg->name[i]] = msg->position[i]; + } robotControlManager_.UpdateJointStates(msg); } @@ -124,11 +148,32 @@ void RobotFsm::JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg) { // 解析消息内容 std::string command = msg->data; + static std::string last_msg=""; + if(last_msg!=command) + last_msg=command; + else + return; if (command == "前进") { - inputDevices_[InputType::JOY]->SetCommand(ControlCommand::FORWARD); + if (isJogMode_ && jogDirection_ == 0) + { + jogIndex_ ++; + std::cout << "switch to jog axis: " << jogIndex_ + 1 << std::endl; + } + else + { + inputDevices_[InputType::JOY]->SetCommand(ControlCommand::FORWARD); + } } else if (command == "后退") { - inputDevices_[InputType::JOY]->SetCommand(ControlCommand::BACKWARD); + if (isJogMode_ && jogDirection_ == 0) + { + jogIndex_ --; + std::cout << "switch to jog axis: " << jogIndex_ + 1 << std::endl; + } + else + { + inputDevices_[InputType::JOY]->SetCommand(ControlCommand::BACKWARD); + } } else if (command == "左转") { inputDevices_[InputType::JOY]->SetCommand(ControlCommand::LEFT); @@ -137,57 +182,129 @@ void RobotFsm::JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg) { inputDevices_[InputType::JOY]->SetCommand(ControlCommand::RIGHT); } else if (command == "上升") { - jogDirection_ = 1; - inputDevices_[InputType::JOY]->SetCommand(ControlCommand::UP); + if (isJogMode_) + { + jogDirection_ = 1; + std::cout << "jog axis in positive direction: " << jogIndex_ + 1 << std::endl; + } + else + { + inputDevices_[InputType::JOY]->SetCommand(ControlCommand::UP); + } } else if (command == "下降") { - jogDirection_ = -1; - inputDevices_[InputType::JOY]->SetCommand(ControlCommand::DOWN); + if (isJogMode_) + { + jogDirection_ = -1; + std::cout << "jog axis in negative direction: " << jogIndex_ + 1 << std::endl; + } + else + { + inputDevices_[InputType::JOY]->SetCommand(ControlCommand::DOWN); + } + } + else if (command == "抬手") { + if (!isJogMode_) + inputDevices_[InputType::JOY]->SetCommand(ControlCommand::HAND_UP); + } + else if (command == "放手") { + if (!isJogMode_) + inputDevices_[InputType::JOY]->SetCommand(ControlCommand::HAND_DOWN); } else if (command == "停止") { - jogDirection_ = 0; - inputDevices_[InputType::JOY]->SetCommand(ControlCommand::STOP); + if (isJogMode_) + { + jogDirection_ = 0; + std::cout << "jog axis stopped: " << jogIndex_ + 1 << std::endl; + } + else + { + inputDevices_[InputType::JOY]->SetCommand(ControlCommand::STOP); + } } else if (command == "A") { - // TODO: need to be checked. - // inputDevices_[InputType::JOY]->SetCommand(ControlCommand::GO_HOME); + if (enableCommandExecuted_) + { + enableCommandExecuted_ = false; + EnableRobot(0); + std::cout<<"enable all motor"<get_logger(), "未知的命令: %s", command.c_str()); } } -void RobotFsm::EnableRobot() +void RobotFsm::ResetFault(double value) { + control_msgs::msg::DynamicInterfaceGroupValues enableMsg; + for (size_t i = 0; i < robotControlManager_.GetMotionParameters().total_joints_; i++) { - control_msgs::msg::DynamicInterfaceGroupValues enableMsg; + std::string tempInterfaceGroup = "joint_" + std::to_string(i+1); + enableMsg.interface_groups.push_back(tempInterfaceGroup); + control_msgs::msg::InterfaceValue tempValue; + tempValue.interface_names = {"reset_fault"}; + tempValue.values = {value}; + enableMsg.interface_values.push_back(tempValue); + } + + PrintMessage(enableMsg); + + gpioPub_->publish(enableMsg); + + if (value == 1) + { + std::cout << "Fault reset" << std::endl; + isFaultReset_ = true; + } +} + +void RobotFsm::EnableRobot(double value) +{ + 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}; + tempValue.values = {value}; enableMsg.interface_values.push_back(tempValue); - - std::cout << "Enable robot " << i + 1 << std::endl; - - gpioPub_->publish(enableMsg); - - sleep(1); } - sleep(2); - - isRobotEnabled_ = true; + gpioPub_->publish(enableMsg); + + isRobotEnabled_ = value == 1? true: false; } void RobotFsm::VoiceCommandCallback(const std_msgs::msg::String::SharedPtr msg) { @@ -233,6 +350,8 @@ void RobotFsm::InitTransitionTable() { transitionTable_[{RobotState::TURNING_RIGHT, ControlCommand::STOP}] = RobotState::STOPPING; transitionTable_[{RobotState::MOVING_UP, ControlCommand::STOP}] = RobotState::STOPPING; transitionTable_[{RobotState::MOVING_DOWN, ControlCommand::STOP}] = RobotState::STOPPING; + transitionTable_[{RobotState::ARM_MOVING_UP, ControlCommand::STOP}] = RobotState::STOPPING; + transitionTable_[{RobotState::ARM_MOVING_DOWN, ControlCommand::STOP}] = RobotState::STOPPING; transitionTable_[{RobotState::HOME_MOVING, ControlCommand::STOP}] = RobotState::STOPPING; transitionTable_[{RobotState::STOPPED, ControlCommand::FORWARD}] = RobotState::MOVING_FORWARD; @@ -241,6 +360,8 @@ 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::HAND_UP}] = RobotState::ARM_MOVING_UP; + transitionTable_[{RobotState::STOPPED, ControlCommand::HAND_DOWN}] = RobotState::ARM_MOVING_DOWN; transitionTable_[{RobotState::STOPPED, ControlCommand::GO_HOME}] = RobotState::HOME_MOVING; transitionTable_[{RobotState::INIT, ControlCommand::FORWARD}] = RobotState::MOVING_FORWARD; @@ -249,6 +370,8 @@ void RobotFsm::InitTransitionTable() { transitionTable_[{RobotState::INIT, ControlCommand::RIGHT}] = RobotState::TURNING_RIGHT; transitionTable_[{RobotState::INIT, ControlCommand::UP}] = RobotState::MOVING_UP; transitionTable_[{RobotState::INIT, ControlCommand::DOWN}] = RobotState::MOVING_DOWN; + transitionTable_[{RobotState::INIT, ControlCommand::HAND_UP}] = RobotState::ARM_MOVING_UP; + transitionTable_[{RobotState::INIT, ControlCommand::HAND_DOWN}] = RobotState::ARM_MOVING_DOWN; transitionTable_[{RobotState::INIT, ControlCommand::GO_HOME}] = RobotState::HOME_MOVING; } @@ -258,10 +381,30 @@ void RobotFsm::AddInputDevice(InputType type, std::unique_ptr devic std::cout << "Added input device: " << inputDevices_[type]->GetDeviceName() << std::endl; } +int RobotFsm::motor_dst(std::string name,double dst){ + double val=curMap_[name]; + double diff=dst-val; + double tempJointValue=0; + float delta=0.0; + if(diff>600){ + delta=120.0; + tempJointValue=val+delta; + }else if(diff<-600){ + delta=-120.0; + tempJointValue=val+delta; + }else{ + return 0; + } + posMsg_.interface_groups.push_back(name); + control_msgs::msg::InterfaceValue tempValue; + tempValue.interface_names = {"position"}; + tempValue.values = {tempJointValue}; + posMsg_.interface_values.push_back(tempValue); + return 0; +} // 状态机主循环 void RobotFsm::ControlLoop() { - // 检查所有输入设备 ControlCommand cmd = ControlCommand::NONE; for (const auto& [type, device] : inputDevices_) { @@ -323,39 +466,38 @@ void RobotFsm::Publish_joint_trajectory() void RobotFsm::Gpio_publish_joint_trajectory() { - static size_t jointIndex = 5; - static auto jointValue = robotControlManager_.GetJointCommands().data; - static double jogValue = jointValue[jointIndex -1]; - - std::cout << jogValue << std::endl; - - if (true) + // jog + if (isJogMode_) { + robotControlManager_.JogAxis(jogIndex_, jogDirection_); + auto cmd_msg = robotControlManager_.GetJointCommands(); + + data_file_ << 0; + + // 2. 保存整个数组数据到txt文件 + if (data_file_.is_open()) { + 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); + std::string tempInterfaceGroup = "joint_" + std::to_string(i+1); positionMsg.interface_groups.push_back(tempInterfaceGroup); - double tempJointValue; - if (i + 1 == jointIndex) - { - jogValue = jogValue + jogDirection_ * 10; - tempJointValue = jogValue; - } - else - { - tempJointValue = jointValue[i]; - } - control_msgs::msg::InterfaceValue tempValue; tempValue.interface_names = {"position"}; - tempValue.values = {tempJointValue}; + tempValue.values = {cmd_msg.data[i]}; positionMsg.interface_values.push_back(tempValue); } - - // 发布消息 + // PrintMessage(positionMsg); + gpioPub_->publish(positionMsg); } else @@ -387,7 +529,7 @@ void RobotFsm::Gpio_publish_joint_trajectory() } // 发布消息 - //gpioPub_->publish(positionMsg); + gpioPub_->publish(positionMsg); } } @@ -438,7 +580,12 @@ void RobotFsm::ExecuteStateAction(double dt) { break; case RobotState::MOVING_DOWN: robotControlManager_.MoveDown(dt); - // 发送下降命令 + break; + case RobotState::ARM_MOVING_UP: + robotControlManager_.ArmMoveUp(dt); + break; + case RobotState::ARM_MOVING_DOWN: + robotControlManager_.ArmMoveDown(dt); break; case RobotState::HOME_MOVING: if(robotControlManager_.GoHome(dt)) @@ -462,8 +609,10 @@ std::string RobotFsm::StateToString(RobotState state) const { case RobotState::TURNING_LEFT: return "TURNING_LEFT"; case RobotState::TURNING_RIGHT: return "TURNING_RIGHT"; case RobotState::MOVING_UP: return "MOVING_UP"; + case RobotState::ARM_MOVING_UP: return "ARM_MOVING_UP"; + case RobotState::ARM_MOVING_DOWN: return "ARM_MOVING_DOWN"; case RobotState::MOVING_DOWN: return "MOVING_DOWN"; case RobotState::HOME_MOVING: return "HOME_MOVING"; default: return "UNKNOWN"; } -} \ No newline at end of file +} diff --git a/HiveCoreR0/src/robot_control/src/rs485_driver.cpp b/HiveCoreR0/src/robot_control/src/rs485_driver.cpp index a1834b5..d25d4ee 100644 --- a/HiveCoreR0/src/robot_control/src/rs485_driver.cpp +++ b/HiveCoreR0/src/robot_control/src/rs485_driver.cpp @@ -47,7 +47,7 @@ namespace robot_control RS485Driver::RS485Driver(size_t maxMotors) : max_motors_(maxMotors), serial_port_(-1), is_open_(false) { - + control_index_ = 0; } RS485Driver::~RS485Driver() @@ -339,35 +339,70 @@ namespace robot_control std::vector command; - // 限制速度范围 - int32_t clamped_speed = speeds[i - 1]; - if (clamped_speed > 1000) clamped_speed = 1000; - if (clamped_speed < -1000) clamped_speed = -1000; + if (i < 5) + { + control_index_ = 1; + // 限制速度范围 + int32_t clamped_speed = speeds[control_index_ - 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); + uint8_t high_byte = (clamped_speed >> 8) & 0xFF; // 高8位 + uint8_t low_byte = clamped_speed & 0xFF; // 低8位 - unsigned char crc8 = CRC8_Table(command.data(), 9); + command.push_back(2); + 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(control_index_) << ", 速度: " << clamped_speed << std::endl; + } + else if ( i > 5) + { + control_index_ = 2; + // 限制速度范围 + int32_t clamped_speed = speeds[control_index_ - 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(3); + 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(control_index_) << ", 速度: " << clamped_speed << std::endl; + } - command.push_back(crc8); - - // 发送命令 - success = sendData(command); - std::cout << "已发送命令到电机 " << static_cast(i) << ", 速度: " << clamped_speed << std::endl; - ++i; - if (i > max_motors_) + // std::cout << "i = " << static_cast(i) << std::endl; + + if (i > 8) { i = 1; return success; diff --git a/HiveCoreR0/src/robot_control/src/wheel_control.cpp b/HiveCoreR0/src/robot_control/src/wheel_control.cpp index fad29b5..7021b9c 100644 --- a/HiveCoreR0/src/robot_control/src/wheel_control.cpp +++ b/HiveCoreR0/src/robot_control/src/wheel_control.cpp @@ -60,9 +60,7 @@ namespace robot_control int32_t rightOmega = rightSpeed; 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 + wheelSpeed_[1] = wheelDirection_[1] * rightOmega; // right_wheel } @@ -94,7 +92,7 @@ namespace robot_control } } - void WheelControl::ExecuteCommand() + bool WheelControl::ExecuteCommand() { CalculateWheelSpeeds(); @@ -106,6 +104,8 @@ namespace robot_control previousWheelSpeed_ = wheelSpeed_; } } + + return isSendFinished_; } } \ No newline at end of file