HiveCoreRD/hivecore_robot_drivers
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ethercat driver finish 0

Browse Source
This commit is contained in:
hehe
2025-12-16 14:24:53 +08:00
parent 51dc86ea4a
commit 874bfaaf8d
8 changed files with 171 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
ecrt_dev/CMakeLists.txt
View File

@@ -11,7 +11,9 @@ find_package(hardware_interface REQUIRED)
find_package(pluginlib REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
###find_package(ethercat_interface REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(trajectory_msgs REQUIRED)
find_package(control_msgs REQUIRED)
find_library(ECRT_LIB
NAMES ethercat ecrt
PATHS /opt/etherlab/lib /usr/local/lib /usr/lib
@@ -40,6 +42,7 @@ ament_target_dependencies(
add_executable(test_motor src/test_motor.cpp)
ament_target_dependencies(test_motor
rclcpp
rclcpp_action
geometry_msgs
sensor_msgs
trajectory_msgs

21
ecrt_dev/config/controllers.yaml
View File

@@ -31,7 +31,14 @@ trajectory_controller:
state_interfaces:
- position
- velocity
state_publish_rate: 200.0
action_monitor_rate: 20.0
allow_partial_joints_goal: false
open_loop_control: true
allow_integration_in_goal_trajectories: true
constraints:
goal_time: 0.5
stopped_velocity_tolerance: 0.02
gpio_command_controller:
ros__parameters:
gpios:
@@ -52,62 +59,50 @@ gpio_command_controller:
- interfaces:
- fault
- enable
- position
joint_2:
- interfaces:
- fault
- enable
- position
joint_3:
- interfaces:
- fault
- enable
- position
joint_4:
- interfaces:
- fault
- enable
- position
joint_5:
- interfaces:
- fault
- enable
- position
joint_6:
- interfaces:
- fault
- enable
- position
joint_7:
- interfaces:
- fault
- enable
- position
joint_8:
- interfaces:
- fault
- enable
- position
joint_9:
- interfaces:
- fault
- enable
- position
joint_10:
- interfaces:
- fault
- enable
- position
joint_11:
- interfaces:
- fault
- enable
- position
joint_12:
- interfaces:
- fault
- enable
- position
state_interfaces:
joint_1:
- interfaces:

1
ecrt_dev/description/motor_drive.ros2_control.xacro
View File

@@ -15,6 +15,7 @@
<joint name="${joint_name}">
<!-- 状态接口（位置/速度/力矩） -->
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="fault"/>
<!-- 命令接口（位置控制/故障重置/使能） -->

3
ecrt_dev/include/ethercat_driver/zer_config.hpp
View File

@@ -1,4 +1,5 @@
#include "ecrt.h"
#include <math.h>
#define NUM_SLAVES 12
#define ZER_VID_PID 0x5a65726f,0x00029252
typedef struct {
@@ -20,6 +21,8 @@ typedef struct {
static zer_offset_t zer_offsets[12];
constexpr double rad_to_count= 524288.0/(2*M_PI);
constexpr double count_to_rad=2*M_PI/524288.0;
#define PDO_ENTRY(alias,position,index) \
{alias,position, ZER_VID_PID, 0x6040, 0, &zer_offsets[index].ctrl_word,NULL}, \

6
ecrt_dev/launch/motor_drive.launch.py
View File

@@ -77,12 +77,12 @@ def generate_launch_description():
arguments=["gpio_command_controller", "-c", "/controller_manager"],
)
position_controller_spawner = Node(
trajectory_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["position_controller", "-c", "/controller_manager"],
arguments=["trajectory_controller", "-c", "/controller_manager"],
)
delay_node=TimerAction(period=1.0,actions=[gpio_controller_spawner])
delay_node=TimerAction(period=1.0,actions=[gpio_controller_spawner,trajectory_controller_spawner])
nodes = [
control_node,
robot_state_pub_node,

1
ecrt_dev/package.xml
View File

@@ -12,6 +12,7 @@
<depend>hardware_interface</depend>
<depend>pluginlib</depend>
<depend>rclcpp</depend>
<depend>rclcpp_lifecycle</depend>
<depend>ethercat_interface</depend>

32
ecrt_dev/src/ethercat_driver.cpp
View File

@@ -401,28 +401,30 @@ void EthercatDriver::readData(){
} else { // do this at 1 Hz
counter = FREQUENCY;
// check for master state (optional)
check_master_state(); //检查主站状态
//check_master_state(); //检查主站状态
// check for slave configuration state(s)
check_slave_config_states(); //检查从站状态
//check_slave_config_states(); //检查从站状态
}
if (!inited) {
for (int i = 0; i < NUM_SLAVES; ++i) {
command[i] = 0x004F;
status[i] = 0x000F;
last_status[i] = status[i];
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0080);
}
inited = 1;
}
for (int i = 0; i < NUM_SLAVES; ++i)
{
uint16_t sw = EC_READ_U16(domain1_pd + zer_offsets[i].status_word);
int32_t pv = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
int32_t vv = EC_READ_S32(domain1_pd + zer_offsets[i].velocity_actual_value);
int32_t pos = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
int32_t vel = EC_READ_S32(domain1_pd + zer_offsets[i].velocity_actual_value);
int16_t tv = EC_READ_S16(domain1_pd + zer_offsets[i].torque_actual_value);
int8_t md = EC_READ_S8 (domain1_pd + zer_offsets[i].modes_of_operation_display);
uint16_t err = EC_READ_U16(domain1_pd + zer_offsets[i].error_code);
hw_joint_states_[i][0]=pv;
hw_joint_states_[i][1]=err;
hw_joint_states_[i][0]=pos*count_to_rad;
hw_joint_states_[i][1]=vel;
hw_joint_states_[i][2]=40960;//err
//hw_joint_states_[i][2]=err;
///RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],state:%.1f,%.1f,0x%04x",i,hw_joint_states_[i][0],hw_joint_states_[i][1],err);
status[i] = sw; //侦测子站状态字变化
@@ -443,8 +445,11 @@ void EthercatDriver::readData(){
///static double last_enable[12];
double enable=hw_joint_commands_[i][1];
if(enable!=1)
if(enable!=1){
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, 0);
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);
continue;
}
//write
if ((status[i] & command[i]) == 0x0001 || (status[i] & command[i]) == 0x0040) {
@@ -465,14 +470,13 @@ void EthercatDriver::readData(){
//printf("VVVVVVVV:%d\n",i);
} else if ((status[i] & command[i]) == 0x0027) {
// ---- 4) 运行态写目标（按模式/运行意图） ----
//if(enable==1)
if(true)
{
//EC_WRITE_U16(domain1_pd + offsets[i].ctrl_word, 0x001F); // 仅在PP模式下需要
switch (mode_cmd) {
case CYCLIC_VELOCITY: // 9
////////EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, vel_cmd);
////EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, vel_cmd);
break;
case CYCLIC_POSITION: { // 8
// ---- 绝对式目标生成，不再用 pv 做增量基准 ----
@@ -486,21 +490,23 @@ void EthercatDriver::readData(){
// 进入/首次运行：把命令位置对齐到“当前实际位置”，避免冲击
if (!csp_inited[i] || last_mode_cmd[i] != CYCLIC_POSITION) {
csp_cmd_pos[i] = pv; // 先对齐
csp_cmd_pos[i] = pos; // 先对齐
vmax_acc[i] = 0;
csp_inited[i] = 1;
}
double target_pos=hw_joint_commands_[i][2];
double target_pos=hw_joint_commands_[i][2]*rad_to_count;
static double last_pos[12];
if(target_pos!=last_pos[i]){
last_pos[i]=target_pos;
}
//RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],cur:%d,dst:%.1f",i,pv,target_pos);
// 期望的“最终绝对目标”
const int32_t goal = target_pos;///g_cmd.target_pos[i].load(std::memory_order_relaxed);
///if(i==0)
///printf("%d,goal:%d,pv:%d,flag:%d\n",i,goal,pv,goal_flag[i]);
static int pos_cnt=0;
//if(pos_cnt++%500==0)
// RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],cur:%d,dst:%.1f",i,pos,target_pos);
// 本周期允许的最大步长（counts/周期），用定点累加保证平均速度上限严格等于 CSP_MAX_VEL_COUNTS_PER_S
vmax_acc[i] += (int64_t)CSP_MAX_VEL_COUNTS_PER_S; // +v [counts/s]
int32_t max_step = (int32_t)(vmax_acc[i] / FREQUENCY); // 下取整，得到本周期可用步长

214
ecrt_dev/src/test_motor.cpp
View File

@@ -4,9 +4,14 @@
#include <time.h>
#include "std_msgs/msg/string.hpp"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
#include "control_msgs/msg/dynamic_interface_group_values.hpp"
#include "control_msgs/action/follow_joint_trajectory.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
namespace fs = std::filesystem;
constexpr double rad_to_count= 524288.0/(2*M_PI);
constexpr double count_to_rad=2*M_PI/524288.0;
class TestMotor : public rclcpp::Node
{
public:
@@ -19,14 +24,14 @@ namespace fs = std::filesystem;
void motor_enable(int id,double enable);
void motor_pos(int id,double pos);
void motor_loop(int motor_id,int cnt);
void motor_ctl(int id,double pos,double reset,double enable);
void motor_action(int id,double angle);
void all_motor();
void ControlLoop();
private:
rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr cmdPub_;
rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr gpioPub_;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr jointStatesSub_;
rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SharedPtr client_;
// 文件流相关
std::ofstream data_file_; // 用于写入数据的文件流
std::string data_file_path_; // 数据文件路径
@@ -57,17 +62,21 @@ TestMotor::TestMotor() : Node("test_motor_node")
std::cout << "TestMotor Node is created!" << std::endl;
// 创建发布者
cmdPub_ = this->create_publisher<trajectory_msgs::msg::JointTrajectory>("/trajectory_controller/joint_trajectory", 10);
//action
client_=rclcpp_action::create_client<control_msgs::action::FollowJointTrajectory>(this,"/trajectory_controller/follow_joint_trajectory");
// 创建发布者
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(&TestMotor::JointStatesCallback, this, std::placeholders::_1));
lastTime_ = this->now(); // 初始化时间
// 创建定时器，每10ms执行一次控制逻辑（频率100Hz）
controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(20000),std::bind(&TestMotor::ControlLoop, this)); // 绑定到新的控制函数);
controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(5000),std::bind(&TestMotor::ControlLoop, this)); // 绑定到新的控制函数);
curMap_.clear();
dstMap_.clear();
//posMsg_.interface_groups.resize(12);
//posMsg_.interface_values.resize(12);
motor_enable(6,1);
motor_enable(13,1);
std::cout << "TestMotor Node is created finished!" << std::endl;
}
@@ -85,7 +94,7 @@ void TestMotor::JointStatesCallback(const sensor_msgs::msg::JointState::SharedPt
for(int i=0;i<msg->name.size();i++)
{
curMap_[msg->name[i]] = msg->position[i];
///std::cout<<msg->name[i]<<":"<<curMap_[msg->name[i]]<<std::endl;
///std::cout<<"cur:"<<msg->name[i]<<":"<<curMap_[msg->name[i]]<<std::endl;
}
}
@@ -117,61 +126,6 @@ void TestMotor::ControlLoop() {
//Gpio_publish_joint_trajectory();
all_motor();
}
void TestMotor::motor_fault(int id,double fault)
{
control_msgs::msg::DynamicInterfaceGroupValues posMsg_;
std::string target_motor="joint_" + std::to_string(id);
std::cout<<"fault:";
for(int i=0;i<12;i++){
std::string tempInterfaceGroup = "joint_" + std::to_string(i+1);
posMsg_.interface_groups.push_back(tempInterfaceGroup);
control_msgs::msg::InterfaceValue tempValue;
//reset
tempValue.interface_names = {"fault"};
if(id==0){
tempValue.values = {fault};
}else{
if(tempInterfaceGroup!=target_motor)
tempValue.values = {0};
else
tempValue.values = {fault};
}
std::cout<<tempInterfaceGroup<<":"<<tempValue.values[0]<<",";
posMsg_.interface_values.push_back(tempValue);
}
std::cout<<std::endl;
gpioPub_->publish(posMsg_);
usleep(10000);
}
void TestMotor::motor_enable(int id,double enable)
{
control_msgs::msg::DynamicInterfaceGroupValues posMsg_;
std::string target_motor="joint_" + std::to_string(id);
std::cout<<"enable:";
for(int i=0;i<12;i++){
std::string tempInterfaceGroup = "joint_" + std::to_string(i+1);
posMsg_.interface_groups.push_back(tempInterfaceGroup);
control_msgs::msg::InterfaceValue tempValue;
//enable
tempValue.interface_names = {"enable"};
if(id==0){
tempValue.values = {enable};
}else{
if(tempInterfaceGroup!=target_motor)
tempValue.values = {0};
else
tempValue.values = {enable};
}
std::cout<<tempInterfaceGroup<<":"<<tempValue.values[0]<<",";
posMsg_.interface_values.push_back(tempValue);
}
std::cout<<std::endl;
gpioPub_->publish(posMsg_);
usleep(10000);
}
void TestMotor::motor_pos(int id,double delta)
{
control_msgs::msg::DynamicInterfaceGroupValues posMsg_;
@@ -197,40 +151,135 @@ void TestMotor::motor_pos(int id,double delta)
gpioPub_->publish(posMsg_);
usleep(10000);
}
void TestMotor::motor_ctl(int id,double pos,double reset,double enable)
void TestMotor::motor_fault(int id,double fault)
{
control_msgs::msg::DynamicInterfaceGroupValues posMsg_;
std::string tempInterfaceGroup = "joint_" + std::to_string(id+1);
posMsg_.interface_groups.push_back(tempInterfaceGroup);
control_msgs::msg::InterfaceValue tempValue;
tempValue.interface_names = {"position"};
tempValue.values = {pos};
posMsg_.interface_values.push_back(tempValue);
tempValue.interface_names = {"fault"};
tempValue.values = {reset};
posMsg_.interface_values.push_back(tempValue);
//enable
tempValue.interface_names = {"enable"};
tempValue.values = {enable};
std::cout<<tempInterfaceGroup<<"ctl:"<<pos<<","<<reset<<","<<enable<<std::endl;
posMsg_.interface_values.push_back(tempValue);
std::string target_motor="joint_" + std::to_string(id);
std::cout<<"fault:";
for(int i=0;i<12;i++){
std::string tempInterfaceGroup = "joint_" + std::to_string(i+1);
posMsg_.interface_groups.push_back(tempInterfaceGroup);
control_msgs::msg::InterfaceValue tempValue;
//reset
tempValue.interface_names = {"fault"};
if(id==0){
tempValue.values = {fault};
}else{
if(tempInterfaceGroup!=target_motor)
tempValue.values = {0};
else
tempValue.values = {fault};
}
std::cout<<tempInterfaceGroup<<":"<<tempValue.values[0]<<",";
posMsg_.interface_values.push_back(tempValue);
}
std::cout<<std::endl;
gpioPub_->publish(posMsg_);
usleep(50000);
}
void TestMotor::motor_enable(int id,double enable)
{
control_msgs::msg::DynamicInterfaceGroupValues posMsg_;
std::string target_motor="joint_" + std::to_string(id);
std::cout<<"enable:";
for(int i=0;i<12;i++){
std::string tempInterfaceGroup = "joint_" + std::to_string(i+1);
posMsg_.interface_groups.push_back(tempInterfaceGroup);
control_msgs::msg::InterfaceValue tempValue;
//enable
tempValue.interface_names = {"enable"};
#if 1
if(i<id)
tempValue.values = {enable};
else
tempValue.values = {0};
#else
if(id==0){
tempValue.values = {enable};
}else{
if(tempInterfaceGroup!=target_motor)
tempValue.values = {0};
else
tempValue.values = {enable};
}
#endif
std::cout<<tempInterfaceGroup<<":"<<tempValue.values[0]<<",";
posMsg_.interface_values.push_back(tempValue);
}
std::cout<<std::endl;
gpioPub_->publish(posMsg_);
usleep(1000000);
}
void TestMotor::motor_action(int id,double delta)
{
//std::cout<<"motor_action:"<<id<<","<<delta<<std::endl;
if(client_->wait_for_action_server(std::chrono::seconds(5))){
auto goal=control_msgs::action::FollowJointTrajectory::Goal();
goal.trajectory.joint_names={"joint_1","joint_2","joint_3","joint_4","joint_5","joint_6","joint_7","joint_8","joint_9","joint_10","joint_11","joint_12"};
trajectory_msgs::msg::JointTrajectoryPoint point;
for(int i=0;i<12;i++){
std::string joint="joint_"+std::to_string(i+1);
//if(id==(i+1))
if(i<12)
point.positions.push_back(curMap_[joint]+delta);
else
point.positions.push_back(curMap_[joint]);
printf("%s: %.2f/%.2f\n",joint.c_str(),curMap_[joint],point.positions[i]);
}
goal.trajectory.points.push_back(point);
auto send_goal_option=rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions();
send_goal_option.goal_response_callback=[this](auto res){
auto goal_handle=res.get();
if(goal_handle){
printf("goal has be accept!!!\n");
}
};
send_goal_option.feedback_callback=[this](auto,auto feedback){
for(int i=0;i<12;i++){
printf("%s,%.3f/%.3f\n",feedback->joint_names[i].c_str(),feedback->desired.positions[i],feedback->actual.positions[i]);
}
};
send_goal_option.result_callback=[this](auto ret){
if(ret.code==rclcpp_action::ResultCode::SUCCEEDED)
printf("action ret succeed\n");
else if(ret.code==rclcpp_action::ResultCode::ABORTED)
printf("action ret aborted\n");
};
client_->async_send_goal(goal,send_goal_option);
}else{
printf("wait action server error\n");
}
}
void TestMotor::motor_loop(int motor_id,int loop_cnt){
std::cout<<"start test:"<<motor_id<<std::endl;
motor_fault(motor_id,1);
motor_enable(motor_id,1);
motor_fault(0,1);
//motor_enable(motor_id,1);
if(loop_cnt%2==0)
motor_pos(motor_id,3000);
//motor_pos(motor_id,3000);
motor_action(motor_id,-0.05);
else
motor_pos(motor_id,-3000);
usleep(1.5*1000000);
//motor_pos(motor_id,-3000);
motor_action(motor_id,0.05);
///usleep(3*1000000);
}
void TestMotor::all_motor(){
loop_cnt+=1;
motor_loop(1,loop_cnt);
static int sw=0;
motor_loop(0,loop_cnt);
#if 0
int mid=loop_cnt%100;
if(mid<13&&mid>0)
motor_loop(mid,sw);
else{
loop_cnt=0;
sw+=1;
motor_loop(1,sw);
}
#endif
#if 0
motor_loop(2,loop_cnt);
motor_loop(3,loop_cnt);
motor_loop(4,loop_cnt);
@@ -242,6 +291,7 @@ void TestMotor::all_motor(){
motor_loop(10,loop_cnt);
motor_loop(11,loop_cnt);
motor_loop(12,loop_cnt);
#endif
}
int main(int argc,char* argv[]){
rclcpp::init(argc,argv);