robofish/CM_DOG
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

3关节

Browse Source
This commit is contained in:
Robofish 2025-04-08 20:42:18 +08:00
parent 593e4a9577
commit 90c944d230
4 changed files with 84 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/rc_msgs/CMakeLists.txt
View File

@ -6,6 +6,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/Ps2Data.msg"
"msg/DataControl.msg"
"msg/DataMotor.msg"
"msg/DataLeg.msg"
)
ament_package()

7
src/rc_msgs/msg/DataLeg.msg Normal file
View File

@ -0,0 +1,7 @@
DataControl data_control_0
DataControl data_control_1
DataControl data_control_2
DataMotor data_motor_0
DataMotor data_motor_1
DataMotor data_motor_2

11
src/unitree_motor_serial_driver/include/unitree_motor_serial_driver/unitree_motor_serial_driver.hpp
View File

@ -6,8 +6,7 @@
#include "rclcpp/rclcpp.hpp"
#include "serialPort/SerialPort.h"
#include "unitreeMotor/unitreeMotor.h"
#include "rc_msgs/msg/data_control.hpp" // 添加 DataControl 消息
#include "rc_msgs/msg/data_motor.hpp" // 添加 DataMotor 消息
#include "rc_msgs/msg/data_leg.hpp"
namespace unitree_motor_serial_driver
{
@ -18,7 +17,7 @@ struct Motor_t
MotorData data;
};
Motor_t motor;
Motor_t motor[3]; // 修改为数组以支持多个电机
class MotorControlNode : public rclcpp::Node
{
@ -27,12 +26,12 @@ public:
private:
void control_motor();
void data_control_callback(const rc_msgs::msg::DataControl::SharedPtr msg);
void data_control_callback(const rc_msgs::msg::DataLeg::SharedPtr msg);
void Motor_Ctrl_Offline(Motor_t *motor_s);
SerialPort serial_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Subscription<rc_msgs::msg::DataControl>::SharedPtr data_control_sub_;
rclcpp::Publisher<rc_msgs::msg::DataMotor>::SharedPtr data_motor_pub_;
rclcpp::Subscription<rc_msgs::msg::DataLeg>::SharedPtr data_control_sub_;
rclcpp::Publisher<rc_msgs::msg::DataLeg>::SharedPtr data_motor_pub_;
int timer_count_ = 0;
};

110
src/unitree_motor_serial_driver/src/unitree_motor_serial_driver.cpp
View File

@ -7,67 +7,97 @@ MotorControlNode::MotorControlNode()
: Node("unitree_motor_serial_driver"), serial_("/dev/ttyACM0")
{
timer_ = this->create_wall_timer(
std::chrono::milliseconds(1), std::bind(&MotorControlNode::control_motor, this));
data_control_sub_ = this->create_subscription<rc_msgs::msg::DataControl>(
"data_control", 10, std::bind(&MotorControlNode::data_control_callback, this, std::placeholders::_1));
data_motor_pub_ = this->create_publisher<rc_msgs::msg::DataMotor>("data_motor", 10);
std::chrono::microseconds(1000), std::bind(&MotorControlNode::control_motor, this));
data_control_sub_ = this->create_subscription<rc_msgs::msg::DataLeg>(
"data_leg_control", 10, std::bind(&MotorControlNode::data_control_callback, this, std::placeholders::_1));
data_motor_pub_ = this->create_publisher<rc_msgs::msg::DataLeg>("data_leg_motor", 10);
}
void MotorControlNode::data_control_callback(const rc_msgs::msg::DataControl::SharedPtr msg)
void MotorControlNode::data_control_callback(const rc_msgs::msg::DataLeg::SharedPtr msg)
{
// 将接收到的 DataControl 消息赋值给 motor_t
motor.cmd.kp = msg->kp;
motor.cmd.kd = msg->kd;
motor.cmd.q = msg->pos * queryGearRatio(MotorType::GO_M8010_6);
motor.cmd.dq = msg->vw * queryGearRatio(MotorType::GO_M8010_6);
motor.cmd.tau = msg->tau;
timer_count_=0;
// 将接收到的 DataLeg 消息分别赋值给 motor_t 数组
for (int i = 0; i < 3; ++i)
{
motor[i].cmd.kp = msg->data_control_0.kp;
motor[i].cmd.kd = msg->data_control_0.kd;
motor[i].cmd.q = msg->data_control_0.pos * queryGearRatio(MotorType::GO_M8010_6);
motor[i].cmd.dq = msg->data_control_0.vw * queryGearRatio(MotorType::GO_M8010_6);
motor[i].cmd.tau = msg->data_control_0.tau;
}
timer_count_ = 0;
}
void MotorControlNode::control_motor()
{
MotorCmd cmd;
MotorData data;
cmd.motorType = MotorType::GO_M8010_6;
data.motorType = MotorType::GO_M8010_6;
cmd.mode = queryMotorMode(MotorType::GO_M8010_6,MotorMode::FOC);
cmd.id = 0;
cmd.kp = motor.cmd.kp;
cmd.kd = motor.cmd.kd;
cmd.q = motor.cmd.q;
cmd.dq = motor.cmd.dq;
cmd.tau = motor.cmd.tau;
rc_msgs::msg::DataLeg motor_msg;
if (timer_count_ > 300)
for (int i = 0; i < 3; ++i)
{
Motor_Ctrl_Offline(&motor);
timer_count_ = 0;
}
else
{
timer_count_++;
}
serial_.sendRecv(&cmd, &data);
MotorCmd cmd;
MotorData data;
cmd.motorType = MotorType::GO_M8010_6;
data.motorType = MotorType::GO_M8010_6;
cmd.mode = queryMotorMode(MotorType::GO_M8010_6, MotorMode::FOC);
cmd.id = i;
cmd.kp = motor[i].cmd.kp;
cmd.kd = motor[i].cmd.kd;
cmd.q = motor[i].cmd.q;
cmd.dq = motor[i].cmd.dq;
cmd.tau = motor[i].cmd.tau;
// 发布 DataMotor 消息
auto motor_msg = rc_msgs::msg::DataMotor();
motor_msg.tau = data.tau;
motor_msg.vw = data.dq;
motor_msg.pos = data.q;
if (timer_count_ > 300)
{
Motor_Ctrl_Offline(&motor[i]);
timer_count_ = 0;
}
else
{
timer_count_++;
}
// 优化串口通信，减少阻塞
if (!serial_.sendRecv(&cmd, &data))
{
RCLCPP_WARN(this->get_logger(), "Failed to communicate with motor %d", i);
continue;
}
// 填充 DataLeg 消息
if (i == 0)
{
motor_msg.data_motor_0.tau = data.tau;
motor_msg.data_motor_0.vw = data.dq / queryGearRatio(MotorType::GO_M8010_6);
motor_msg.data_motor_0.pos = data.q / queryGearRatio(MotorType::GO_M8010_6);
}
else if (i == 1)
{
motor_msg.data_motor_1.tau = data.tau;
motor_msg.data_motor_1.vw = data.dq / queryGearRatio(MotorType::GO_M8010_6);
motor_msg.data_motor_1.pos = data.q / queryGearRatio(MotorType::GO_M8010_6);
}
else if (i == 2)
{
motor_msg.data_motor_2.tau = data.tau;
motor_msg.data_motor_2.vw = data.dq / queryGearRatio(MotorType::GO_M8010_6);
motor_msg.data_motor_2.pos = data.q / queryGearRatio(MotorType::GO_M8010_6);
}
}
// 发布 DataLeg 消息
data_motor_pub_->publish(motor_msg);
}
void MotorControlNode::Motor_Ctrl_Offline(Motor_t *motor_s)
{
motor_s->cmd.kp = 0.0;
motor_s->cmd.kd = 0.0;
motor_s->cmd.kd = 0.01;
motor_s->cmd.q = 0.0;
motor_s->cmd.dq = 0.0;
motor_s->cmd.tau = 0.0;
}
}// namespace unitree_motor_serial_driver
} // namespace unitree_motor_serial_driver
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);