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

orin ethercat 1ms&12 motor ok

Browse Source
This commit is contained in:
hehe
2026-01-05 16:53:13 +08:00
parent 4b329a36e0
commit 7144e76127
8 changed files with 262 additions and 269 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
ecrt_dev/CMakeLists.txt
View File

@@ -35,6 +35,7 @@ ament_target_dependencies(
pluginlib
rclcpp
rclcpp_lifecycle
rclcpp_action
)
@@ -50,7 +51,16 @@ ament_target_dependencies(test_motor
)
install(TARGETS test_motor DESTINATION lib/${PROJECT_NAME})
#add_executable(key_ctl src/key_ctl.cpp)
#ament_target_dependencies(key_ctl
# rclcpp
# rclcpp_action
# geometry_msgs
# sensor_msgs
# trajectory_msgs
# control_msgs
#)
#install(TARGETS key_ctl DESTINATION lib/${PROJECT_NAME})
# Causes the visibility macros to use dllexport rather than dllimport,
# which is appropriate when building the dll but not consuming it.

52
ecrt_dev/config/controllers.yaml
View File

@@ -1,7 +1,8 @@
controller_manager:
ros__parameters:
update_rate: 1000 # Hz
rt_thread_core: 0
rt_thread_priority: 90
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
@@ -31,6 +32,7 @@ trajectory_controller:
state_interfaces:
- position
- velocity
allow_nonzero_velocity_at_trajectory_end: true
state_publish_rate: 200.0
action_monitor_rate: 20.0
allow_partial_joints_goal: false
@@ -56,87 +58,87 @@ gpio_command_controller:
- joint_12
command_interfaces:
joint_1:
- interfaces:
interfaces:
- fault
- enable
joint_2:
- interfaces:
interfaces:
- fault
- enable
joint_3:
- interfaces:
interfaces:
- fault
- enable
joint_4:
- interfaces:
interfaces:
- fault
- enable
joint_5:
- interfaces:
interfaces:
- fault
- enable
joint_6:
- interfaces:
interfaces:
- fault
- enable
joint_7:
- interfaces:
interfaces:
- fault
- enable
joint_8:
- interfaces:
interfaces:
- fault
- enable
joint_9:
- interfaces:
interfaces:
- fault
- enable
joint_10:
- interfaces:
interfaces:
- fault
- enable
joint_11:
- interfaces:
interfaces:
- fault
- enable
joint_12:
- interfaces:
interfaces:
- fault
- enable
state_interfaces:
joint_1:
- interfaces:
interfaces:
- fault
joint_2:
- interfaces:
interfaces:
- fault
joint_3:
- interfaces:
interfaces:
- fault
joint_4:
- interfaces:
interfaces:
- fault
joint_5:
- interfaces:
interfaces:
- fault
joint_6:
- interfaces:
interfaces:
- fault
joint_7:
- interfaces:
interfaces:
- fault
joint_8:
- interfaces:
interfaces:
- fault
joint_9:
- interfaces:
interfaces:
- fault
joint_10:
- interfaces:
interfaces:
- fault
joint_11:
- interfaces:
interfaces:
- fault
joint_12:
- interfaces:
interfaces:
- fault

25
ecrt_dev/include/ethercat_driver/ethercat_driver.hpp
View File

@@ -30,6 +30,10 @@
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "ethercat_driver/visibility_control.h"
#include <pthread.h>
#include <sched.h>
#include <sys/mman.h>
//#include "ethercat_interface/ec_slave.hpp"
//#include "ethercat_interface/ec_master.hpp"
#include "zer_config.hpp"
@@ -73,14 +77,14 @@ public:
void writeData();
void check_master_state();
void check_domain1_state();
void check_slave_config_states();
bool check_slave_config_states();
void set_motor_enable(int id,bool enable){
if(id>0&&id<13){
if(id>0&&id<NUM_SLAVES+1){
motor_enable_arr[id-1].store(enable);
}
};
bool get_motor_enable(int id){
if(id>0&&id<13){
if(id>0&&id<NUM_SLAVES+1){
return motor_enable_arr[id-1].load();
}
return false;
@@ -117,8 +121,7 @@ private:
std::mutex ec_mutex_;
bool activated_;
#define FREQUENCY 1000
#define NSEC_PER_SEC (1000000000L)
#define FREQUENCY 1000
#define CSP_MAX_VEL_COUNTS_PER_S 65536
#define CSP_POS_DEADBAND 10 //CSP允许的误差(计数)
#define CLOCK_TO_USE CLOCK_MONOTONIC
@@ -127,6 +130,7 @@ private:
#define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + (B).tv_nsec - (A).tv_nsec)
#define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)
const struct timespec cycletime = {0, PERIOD_NS};
uint16_t clear_cmd[NUM_SLAVES];
uint16_t command[NUM_SLAVES]; //状态字掩码
uint16_t status[NUM_SLAVES]; //状态字
uint16_t last_status[NUM_SLAVES]; //上个循环的状态字
@@ -135,6 +139,17 @@ private:
int inited = 0; //初始化
unsigned int counter = 0;
unsigned int sync_ref_counter = 0;
bool has_clear_all=false;
enum ErrState{
ERR_NONE,
ERR_ACK,
ERR_ACK_WAIT,
ERR_CLEAR,
ERR_CLEAR_WAIT,
ERR_FIN,
ERR_FIN_WAIT
};
ErrState errStaArr[NUM_SLAVES]={ERR_NONE};
};
} // namespace ethercat_driver

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

@@ -19,10 +19,11 @@ typedef struct {
unsigned int reserved2; // 0xf0ff:00
} zer_offset_t;
static zer_offset_t zer_offsets[12];
static zer_offset_t zer_offsets[NUM_SLAVES];
constexpr double rad_to_count= 524288.0/(2*M_PI);
constexpr double count_to_rad=2*M_PI/524288.0;
constexpr double speed_to_count=262144.0;
#define PDO_ENTRY(alias,position,index) \
{alias,position, ZER_VID_PID, 0x6040, 0, &zer_offsets[index].ctrl_word,NULL}, \

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

@@ -83,6 +83,7 @@ def generate_launch_description():
arguments=["trajectory_controller", "-c", "/controller_manager"],
)
delay_node=TimerAction(period=1.0,actions=[gpio_controller_spawner,trajectory_controller_spawner])
#delay_node=TimerAction(period=1.0,actions=[gpio_controller_spawner])
nodes = [
control_node,
robot_state_pub_node,
@@ -90,6 +91,4 @@ def generate_launch_description():
delay_node,
]#position_controller_spawner,
return LaunchDescription(
declared_arguments +
nodes)
return LaunchDescription( declared_arguments + nodes)

2
ecrt_dev/package.xml
View File

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

378
ecrt_dev/src/ethercat_driver.cpp
View File

@@ -1,4 +1,9 @@
#include <sys/mman.h> // 用于 mlockall
#include <pthread.h> // 用于线程优先级设置
#include <sched.h> // 用于调度策略
#include <cstring> // 用于 strerror
#include <unistd.h> // 用于 getpid
#include <sys/resource.h>
#include "ethercat_driver/ethercat_driver.hpp"
#include <tinyxml2.h>
#include <string>
@@ -6,16 +11,36 @@
#include"ecrt.h"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"
#include <chrono>
namespace ethercat_driver
{
CallbackReturn EthercatDriver::on_init(
const hardware_interface::HardwareInfo & info)
CallbackReturn EthercatDriver::on_init(const hardware_interface::HardwareInfo & info)
{
if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
return CallbackReturn::ERROR;
}
cpu_set_t cpuset;
CPU_ZERO(&cpuset);
int cpu_core=2;
CPU_SET(cpu_core, &cpuset);
if (pthread_setaffinity_np(pthread_self(), sizeof(cpu_set_t), &cpuset) != 0) {
RCLCPP_ERROR(rclcpp::get_logger("hehe"), "Failed to set CPU affinity to core %d!", cpu_core);
return CallbackReturn::ERROR;
}
struct sched_param param;
param.sched_priority = 99;
if (pthread_setschedparam(pthread_self(), SCHED_FIFO, &param) != 0) {
RCLCPP_ERROR(rclcpp::get_logger("EthercatDriver"),
"Failed to set real-time priority: %s", strerror(errno));
return CallbackReturn::ERROR;
}
if (mlockall(MCL_CURRENT | MCL_FUTURE) != 0) {
RCLCPP_WARN(rclcpp::get_logger("EthercatDriver"),
"Failed to lock memory: %s", strerror(errno));
}
const std::lock_guard<std::mutex> lock(ec_mutex_);
activated_ = false;
for(int i=0;i<NUM_SLAVES;i++){
@@ -60,7 +85,7 @@ CallbackReturn EthercatDriver::on_init(
info_.gpios[g].command_interfaces.size(),
std::numeric_limits<double>::quiet_NaN());
}
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "joints.size:%d,%d,%d",info_.joints.size(),info_.joints[0].state_interfaces.size(),info_.joints[1].state_interfaces.size());
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "joints.size:%ld,%ld,%ld",info_.joints.size(),info_.joints[0].state_interfaces.size(),info_.joints[1].state_interfaces.size());
for (uint j = 0; j < info_.joints.size(); j++) {
//RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "joints");
// check all joints for EC modules and load into ec_modules_
@@ -78,7 +103,7 @@ RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "joints.size:%d,%d,%d",info_.j
}
}
}
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "gpios.size:%d",info_.gpios.size());
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "gpios.size:%ld",info_.gpios.size());
for (uint g = 0; g < info_.gpios.size(); g++) {
//RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "gpios");
// check all gpios for EC modules and load into ec_modules_
@@ -96,7 +121,7 @@ RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "joints.size:%d,%d,%d",info_.j
}
}
}
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "sensors.size:%d",info_.joints.size());
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "sensors.size:%ld",info_.joints.size());
for (uint s = 0; s < info_.sensors.size(); s++) {
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "sensors");
// check all sensors for EC modules and load into ec_modules_
@@ -139,7 +164,8 @@ RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "joints.size:%d,%d,%d",info_.j
std::cout << "Failed PDO config " << i << std::endl;
}
}
ecrt_slave_config_dc(sc[i], 0x0300, PERIOD_NS, 200, 0, 0);
ecrt_slave_config_dc(sc[i], 0x0300, PERIOD_NS, 5000, 0, 0);
//ecrt_slave_config_dc(sc[i], 0x0700, PERIOD_NS, 5000, PERIOD_NS/2, 0);
}
ecrt_domain_reg_pdo_entry_list(domain1,zer_domain1_regs);
if (ecrt_master_activate(master)) {
@@ -165,12 +191,12 @@ CallbackReturn EthercatDriver::on_configure(
std::vector<hardware_interface::StateInterface>
EthercatDriver::export_state_interfaces()
{
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "#######export_state_interfaces:%d,%d,%d",info_.joints.size(),info_.sensors.size(),info_.gpios.size());
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "#######export_state_interfaces:%ld,%ld,%ld",info_.joints.size(),info_.sensors.size(),info_.gpios.size());
std::vector<hardware_interface::StateInterface> state_interfaces;
// export joint state interface
for (uint j = 0; j < info_.joints.size(); j++) {
for (uint i = 0; i < info_.joints[j].state_interfaces.size(); i++) {
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"export:%s,%s\n",info_.joints[j].name.c_str(),info_.joints[j].state_interfaces[i].name.c_str());
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"export:%s,%s",info_.joints[j].name.c_str(),info_.joints[j].state_interfaces[i].name.c_str());
state_interfaces.emplace_back(
hardware_interface::StateInterface(
info_.joints[j].name,
@@ -204,7 +230,7 @@ EthercatDriver::export_state_interfaces()
std::vector<hardware_interface::CommandInterface>
EthercatDriver::export_command_interfaces()
{
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "#######export_command_interfaces:%d",info_.joints.size());
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "#######export_command_interfaces:%ld",info_.joints.size());
std::vector<hardware_interface::CommandInterface> command_interfaces;
// export joint command interface
///std::vector<double> test;
@@ -220,7 +246,7 @@ RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "#######export_command_interfa
for (uint j = 0; j < info_.joints.size(); j++) {
////RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"size:%d\n",info_.joints[j].command_interfaces.size());
for (uint i = 0; i < info_.joints[j].command_interfaces.size(); i++) {
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"joints:%s,%s\n",info_.joints[j].name.c_str(),info_.joints[j].command_interfaces[i].name.c_str());
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"joints:%s,%s",info_.joints[j].name.c_str(),info_.joints[j].command_interfaces[i].name.c_str());
command_interfaces.emplace_back(
hardware_interface::CommandInterface(
info_.joints[j].name,
@@ -241,7 +267,7 @@ RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "#######export_command_interfa
// export gpio command interface
for (uint g = 0; g < info_.gpios.size(); g++) {
for (uint i = 0; i < info_.gpios[g].command_interfaces.size(); i++) {
printf("gpios:%s,%s\n",info_.gpios[g].name,info_.gpios[g].command_interfaces[i].name);
///printf("gpios:%s,%s\n",info_.gpios[g].name,info_.gpios[g].command_interfaces[i].name);
command_interfaces.emplace_back(
hardware_interface::CommandInterface(
info_.gpios[g].name,
@@ -266,7 +292,7 @@ CallbackReturn EthercatDriver::on_activate(
} else {
control_frequency_ = std::stod(info_.hardware_parameters["control_frequency"]);
}
control_frequency_=1000;
control_frequency_=FREQUENCY;
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"control_frequency_:%d\n",control_frequency_);
if (control_frequency_ < 0) {
RCLCPP_FATAL(
@@ -275,9 +301,13 @@ CallbackReturn EthercatDriver::on_activate(
}
clock_gettime(CLOCK_TO_USE, &wakeupTime);
for(int i=0;i<NUM_SLAVES;i++){
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"slave:%d,ctl offset:%p,sta offset:%p\n",i,zer_offsets[i].ctrl_word,zer_offsets[i].status_word);
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"slave:%d,ctl offset:%d,sta offset:%d\n",i,zer_offsets[i].ctrl_word,zer_offsets[i].status_word);
}
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"activate finish");
for(int i=0;i<info_.joints.size();i++){
for(int j=0;j<info_.joints[i].command_interfaces.size();j++)
hw_joint_commands_[i][j]=0.0;
}
activated_ = true;
return CallbackReturn::SUCCESS;
}
@@ -314,7 +344,12 @@ hardware_interface::return_type EthercatDriver::read(
//printf("read data...\n");
if (lock.owns_lock() && activated_) {
////master_.readData();
auto start=std::chrono::high_resolution_clock::now();
readData();
auto end=std::chrono::high_resolution_clock::now();
auto diff=end-start;
if(diff.count()>500000)
RCLCPP_INFO(rclcpp::get_logger("Ethercat"),"diff:%ld",diff.count());
}
return hardware_interface::return_type::OK;
}
@@ -346,20 +381,22 @@ void EthercatDriver::check_master_state(void)
master_state = ms;
}
void EthercatDriver::check_slave_config_states(void)
bool EthercatDriver::check_slave_config_states(void)
{
ec_slave_config_state_t s;
bool all_op=true;
for (int i = 0; i < NUM_SLAVES; ++i) {
errStaArr[i]=ERR_NONE;
if (!sc[i]) continue;
ecrt_slave_config_state(sc[i], &s);
if (s.al_state != sc_state[i].al_state)
//printf("[S%02d] State 0x%02X.\n", i, s.al_state);
if (s.online != sc_state[i].online)
//printf("[S%02d] %s.\n", i, s.online ? "online" : "offline");
if (s.operational != sc_state[i].operational)
//printf("[S%02d] %soperational.\n", i, s.operational ? "" : "Not ");
if ((s.al_state != sc_state[i].al_state)||(s.online != sc_state[i].online)||(s.operational != sc_state[i].operational)){
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"[S%02d] %s,State 0x%02X,OP %s", i, s.online ? "online" : "offline",s.al_state,s.operational ? "OK" : "ERR");
}
sc_state[i] = s;
if(!s.operational)
all_op=false;
}
return all_op;
}
struct timespec EthercatDriver::timespec_add(struct timespec time1, struct timespec time2)
{
@@ -377,16 +414,17 @@ struct timespec EthercatDriver::timespec_add(struct timespec time1, struct times
}
void EthercatDriver::readData(){
#if 0
static int print_cnt=0;
if(print_cnt++%1000==0){
print_cnt+=1;
#if 0
if(print_cnt%1000==0){
for(int i=0;i<info_.joints.size();i++){
printf("[%d]%s:",i,info_.joints[i].name.c_str());
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"[%d]%s:",i,info_.joints[i].name.c_str());
for(int j=0;j<info_.joints[i].command_interfaces.size();j++)
printf("%.2f,",hw_joint_commands_[i][j]);
printf("\n");
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"%.2f,",hw_joint_commands_[i][j]);
//RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"\n");
}
printf("\n");
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"\n");
}
#endif
wakeupTime = timespec_add(wakeupTime, cycletime);
@@ -394,23 +432,38 @@ void EthercatDriver::readData(){
ecrt_master_application_time(master, TIMESPEC2NS(wakeupTime)); // 更新主站应用时间
ecrt_master_receive(master); //接收 EtherCAT 帧
ecrt_domain_process(domain1); //处理域数据
// check process data state (optional)
check_domain1_state(); //检查域状态
if (counter) {
counter--;
} 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(); //检查从站状态
bool all_op=check_slave_config_states(); //检查从站状态
if(all_op){
if(!has_clear_all){
has_clear_all=true;
for (int i = 0; i < NUM_SLAVES; ++i) {
uint16_t err = EC_READ_U16(domain1_pd + zer_offsets[i].error_code);
if(err>0){
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],first code 0x%04x",i,err);
clear_cmd[i]=1;//
has_clear_all=false;
}else{
clear_cmd[i]=0;
}
}
}
}else{
return;
}
}
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);
last_status[i] = status[i];
}
inited = 1;
}
@@ -422,36 +475,77 @@ void EthercatDriver::readData(){
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]=pos*count_to_rad;
hw_joint_states_[i][1]=vel;
hw_joint_states_[i][2]=40960;//err
hw_joint_states_[i][0]=std::round(pos*count_to_rad * 1000.0) / 1000.0;
hw_joint_states_[i][1]=std::round(vel);
hw_joint_states_[i][2]=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);
///
//if(print_cnt%1000==0)
// RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],p:%d,v:%d,0x%04x,%d",i,pos,vel,err,(sw&0x006f)==0x0008);
status[i] = sw; //侦测子站状态字变化
if (status[i] != last_status[i]) {
last_status[i] = status[i];
}
#if 1
if ((status[i]&0x006f)==0x0008){
uint16_t err = EC_READ_U16(domain1_pd + zer_offsets[i].error_code);
double fault=hw_joint_commands_[i][0];
if(fault==1.0&&err>0){
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],fault code 0x%04x",i,err);
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0080);
if(err>0){
double fault=hw_joint_commands_[i][0];
if(fault==1.0||clear_cmd[i]==1){
if(print_cnt%1000==0)
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],%.1f/%d,fault code 0x%04x",i,fault,clear_cmd[i],err);
if(errStaArr[i]==ERR_NONE){
errStaArr[i]=ERR_ACK;
}
}
}
}
///static double last_enable[12];
//clear fault
if(errStaArr[i]==ERR_ACK){
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0000);
errStaArr[i]=ERR_ACK_WAIT;
///RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],0x%04x,ERR_ACK %d",i,err,(sw&0x006f)==0x0008);
}else if(errStaArr[i]==ERR_ACK_WAIT){
//RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],0x%04x,ERR_ACK_WAIT %d",i,err,(sw&0x006f)==0x0008);
if ((status[i]&0x006f)==0x0008){
}
errStaArr[i]=ERR_CLEAR;
}else if(errStaArr[i]==ERR_CLEAR){
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0080);
errStaArr[i]=ERR_CLEAR_WAIT;
//RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],%d,ERR_CLEAR",i,clear_cmd[i]);
}else if(errStaArr[i]==ERR_CLEAR_WAIT){
//RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],0x%04x,ERR_CLEAR_WAIT %d",i,err,(sw&0x006f)==0x0008);
errStaArr[i]=ERR_FIN;
}else if(errStaArr[i]==ERR_FIN){
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0006);
//RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],0x%04x,ERR_FIN %d",i,err,(sw&0x006f)==0x0008);
errStaArr[i]=ERR_FIN_WAIT;
}else if(errStaArr[i]==ERR_FIN_WAIT){
//RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],0x%04x,ERR_FIN_WAIT %d",i,err,(sw&0x006f)==0x0008);
if((sw&0x006f)==0x0008){
errStaArr[i]=ERR_ACK;
}else{
errStaArr[i]=ERR_NONE;
///continue;
}
}
#endif
if(errStaArr[i]!=ERR_NONE)
continue;
double enable=hw_joint_commands_[i][1];
if(enable!=1){
//if(print_cnt%500==0)
// RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"enable:%d,%.1f,%.2f",i,enable,hw_joint_commands_[i][2]);
if(enable!=1.0){
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, 0);
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0006);
continue;
}
//write
//if(print_cnt%500==0)
// RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"write:%d,%.3f,0x%04x",i,hw_joint_commands_[i][2],status[i] & command[i]);
if ((status[i] & command[i]) == 0x0001 || (status[i] & command[i]) == 0x0040) {
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0006); // Ready to switch on
command[i] = 0x006F;
@@ -480,8 +574,6 @@ void EthercatDriver::readData(){
break;
case CYCLIC_POSITION: { // 8
// ---- 绝对式目标生成，不再用 pv 做增量基准 ----
////printf("%02d-->%d,%d,%d\n",i,hw_joint_commands_[i][0],hw_joint_commands_[i][1],hw_joint_commands_[i][2]);
///break;
// 每轴记住“上次下发的命令位置”
static int32_t csp_cmd_pos[NUM_SLAVES]; // 上次下发到 0x607A 的值
static uint8_t csp_inited[NUM_SLAVES] = {0}; // 是否已初始化
@@ -495,16 +587,24 @@ void EthercatDriver::readData(){
csp_inited[i] = 1;
}
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;
if(target_pos<0.001)
continue;
// static double last_pos[12];
// if(target_pos!=last_pos[i]){f
// last_pos[i]=target_pos;
// }
double diff=std::abs(target_pos-234567.0);
if(diff>40000){
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "value too big [%d],cur:%d,dst:%.1f",i,pos,target_pos);
continue;
}else{
//if(print_cnt%500==0)
// RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "value SET [%d],cur:%d,dst:%.1f",i,pos,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;
const int32_t goal = target_pos;
///
//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
@@ -530,6 +630,7 @@ void EthercatDriver::readData(){
// if (std::abs(follow_err) > FE_LIMIT) {
// // 例如：冻结或减半 max_step，给驱动时间追上
// }
#if 1
// 下发“绝对目标”（不是增量）
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, csp_cmd_pos[i]);
@@ -577,167 +678,6 @@ void EthercatDriver::readData(){
ecrt_domain_queue(domain1); //排队域数据
ecrt_master_send(master); //发送ETHERCAT帧
}
void EthercatDriver::writeData(){
///return;
for (int i = 0; i < NUM_SLAVES; ++i){
int8_t mode_cmd=8;
int32_t pv = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
uint16_t sw = EC_READ_U16(domain1_pd + zer_offsets[i].status_word);
status[i] = sw; //侦测子站状态字变化
if (status[i] != last_status[i]) {
// printf("[S%02d] 状态改变为: 0x%04X\n", i, status[i]);
last_status[i] = status[i];
}
///printf("%d error status:%04x,%04x\n",i,status[i],status[i]&0x006f);
#if 0
static double last_reset[12];
if(last_reset[i]!=reset){
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],reset:%.1f",i,reset);
last_reset[i]=reset;
}
#endif
if ((status[i]&0x006f)==0x0008){
static int err_cnt=0;
uint16_t err = EC_READ_U16(domain1_pd + zer_offsets[i].error_code);
if(err_cnt++%500==0)
printf("%d error status:%04x,err:%04x\n",i,status[i],err);
}
if ((status[i] & command[i]) == 0x0001 || (status[i] & command[i]) == 0x0040) {
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0006); // Ready to switch on
command[i] = 0x006F;
///printf("SS:%d\n",i);
} else if ((status[i] & command[i]) == 0x0021) {
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007); // Switched on
command[i] = 0x006F;
// 同步当前位置为目标位置
int32_t pv2 = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv2);
/////g_cmd.target_pos[i].store(pv2, std::memory_order_relaxed);
//printf("TTTTTTTTT:%d\n",i);
} else if ((status[i] & command[i]) == 0x0023) {
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x000f); // Operation enabled
command[i] = 0x006F;
//printf("VVVVVVVV:%d\n",i);
} else if ((status[i] & command[i]) == 0x0027) {
// ---- 4) 运行态写目标（按模式/运行意图） ----
static double last_enable[12];
double enable=hw_joint_commands_[i][2];
#if 0
if(last_enable[i]!=enable){
//std::cout<<i<<",enable:"<<enable<<std::endl;
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],enable:%.1f",i,enable);
last_enable[i]=enable;
}
#endif
if(enable==1)
{
//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);
break;
case CYCLIC_POSITION: { // 8
// ---- 绝对式目标生成，不再用 pv 做增量基准 ----
////printf("%02d-->%d,%d,%d\n",i,hw_joint_commands_[i][0],hw_joint_commands_[i][1],hw_joint_commands_[i][2]);
///break;
// 每轴记住“上次下发的命令位置”
static int32_t csp_cmd_pos[NUM_SLAVES]; // 上次下发到 0x607A 的值
static uint8_t csp_inited[NUM_SLAVES] = {0}; // 是否已初始化
// 定点限速累计器：解决 v_max < FREQUENCY 时整除为 0 的问题
static int64_t vmax_acc[NUM_SLAVES] = {0}; // 单位：counts/s 累加器
// 进入/首次运行：把命令位置对齐到“当前实际位置”，避免冲击
if (!csp_inited[i] || last_mode_cmd[i] != CYCLIC_POSITION) {
csp_cmd_pos[i] = pv; // 先对齐
vmax_acc[i] = 0;
csp_inited[i] = 1;
}
double target_pos=hw_joint_commands_[i][0];
static double last_pos[12];
if(target_pos!=last_pos[i]){
last_pos[i]=target_pos;
}
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],pos:%.1f",i,target_pos);
// 期望的“最终绝对目标”
const int32_t goal = pv;///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]);
// 本周期允许的最大步长（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); // 下取整，得到本周期可用步长
vmax_acc[i] -= (int64_t)max_step * FREQUENCY; // 保留余数到下周期
// 相对“命令轨迹”的误差（不是相对 pv）
const int32_t err_cmd = goal - csp_cmd_pos[i];
// 死区：足够近就贴合到 goal
if (err_cmd > -CSP_POS_DEADBAND && err_cmd < CSP_POS_DEADBAND) {
csp_cmd_pos[i] = goal;
} else {
// 限速迈步：命令轨迹只按时间往 goal 逼近
if (err_cmd > 0) csp_cmd_pos[i] += (err_cmd > max_step) ? max_step : err_cmd;
else csp_cmd_pos[i] += (err_cmd < -max_step) ? -max_step : err_cmd;
}
// 可选：跟随误差防护（避免 following error 过大时继续“加命令”）
// const int32_t follow_err = csp_cmd_pos[i] - pv;
// const int32_t FE_LIMIT = (int32_t) (你的驱动跟随误差阈值); // 参考驱动参数
// if (std::abs(follow_err) > FE_LIMIT) {
// // 例如：冻结或减半 max_step，给驱动时间追上
// }
#if 0
// 下发“绝对目标”（不是增量）
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, csp_cmd_pos[i]);
// 回显给上层（/joint_states_cmd 用）
//////g_state.pos_cmd[i].store(csp_cmd_pos[i], std::memory_order_relaxed);
#endif
break;
}
case CYCLIC_TORQUE: // 10
///////EC_WRITE_S16(domain1_pd + zer_offsets[i].target_torque, tor_cmd);
break;
default:
// 未知模式：默认速度模式安全置 0
//EC_WRITE_S32(domain1_pd + offsets[i].target_velocity, 0);
break;
}
} else {
// run_enable=false：清零目标
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);
// CSV 安全：速度清零
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, 0);
// CSP 安全：目标位置=当前位置 -> 立即“冻结”
int32_t pv_hold = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv_hold);
// 可选：扭矩模式也清零（最好维持急停状态，待修改）
EC_WRITE_S16(domain1_pd + zer_offsets[i].target_torque, 0);
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);//切换 switched on 状态
}
}
}
if (sync_ref_counter) {
sync_ref_counter--;
} else {
sync_ref_counter = 1; // sync every 2 cycle
clock_gettime(CLOCK_TO_USE, &time);
ecrt_master_sync_reference_clock_to(master, TIMESPEC2NS(time)); //每隔一个周期同步时钟
}
ecrt_master_sync_slave_clocks(master);
// send process data
ecrt_domain_queue(domain1); //排队域数据
ecrt_master_send(master); //发送ETHERCAT帧
}
hardware_interface::return_type EthercatDriver::write(
const rclcpp::Time & /*time*/,
const rclcpp::Duration & /*period*/)

54
ecrt_dev/src/test_motor.cpp
View File

@@ -2,6 +2,10 @@
#include <filesystem>
#include <fstream> // 添加这行来支持文件流操作
#include <time.h>
#include <termios.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include "std_msgs/msg/string.hpp"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
@@ -27,6 +31,7 @@ constexpr double count_to_rad=2*M_PI/524288.0;
void motor_action(int id,double angle);
void all_motor();
void ControlLoop();
bool key_available();
private:
rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr cmdPub_;
rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr gpioPub_;
@@ -38,7 +43,7 @@ constexpr double count_to_rad=2*M_PI/524288.0;
rclcpp::TimerBase::SharedPtr controlTimer_;
rclcpp::Time lastTime_; // 移至类成员
struct termios original_termios_;
bool isRunning_;
bool isPaused_;
bool isFinished_;
@@ -75,8 +80,14 @@ TestMotor::TestMotor() : Node("test_motor_node")
dstMap_.clear();
//posMsg_.interface_groups.resize(12);
//posMsg_.interface_values.resize(12);
motor_enable(6,1);
motor_enable(13,1);
std::cout << "start enblex ..." << std::endl;
// motor_enable(3,1);
// sleep(2);
// motor_enable(6,1);
// sleep(2);
// //motor_enable(9,1);
// sleep(2);
//motor_enable(13,1);
std::cout << "TestMotor Node is created finished!" << std::endl;
}
@@ -84,7 +95,24 @@ TestMotor::~TestMotor()
{
std::cout << "Robot controller stopped." << std::endl;
}
bool TestMotor::key_available() {
struct termios new_termios;
memcpy(&new_termios, &original_termios_, sizeof(termios));
// 将终端设置为非规范模式，立即读取输入
new_termios.c_lflag &= ~(ICANON | ECHO);
new_termios.c_cc[VMIN] = 0; // 读取时不需要等待最小字符数
new_termios.c_cc[VTIME] = 0; // 读取等待时间为0
tcsetattr(STDIN_FILENO, TCSANOW, &new_termios);
int bytes_waiting;
ioctl(STDIN_FILENO, FIONREAD, &bytes_waiting);
// 立即恢复原始终端设置
tcsetattr(STDIN_FILENO, TCSANOW, &original_termios_);
return bytes_waiting > 0;
}
void TestMotor::JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg)
{
if (!msg) { // 检查消息是否有效
@@ -253,22 +281,22 @@ void TestMotor::motor_action(int id,double delta)
}
void TestMotor::motor_loop(int motor_id,int loop_cnt){
std::cout<<"start test:"<<motor_id<<std::endl;
motor_fault(0,1);
//motor_enable(motor_id,1);
////motor_fault(0,1);
motor_enable(13,1);
if(loop_cnt%2==0)
//motor_pos(motor_id,3000);
motor_action(motor_id,-0.05);
motor_action(motor_id,-0.04);
else
//motor_pos(motor_id,-3000);
motor_action(motor_id,0.05);
motor_action(motor_id,0.04);
///usleep(3*1000000);
}
void TestMotor::all_motor(){
loop_cnt+=1;
static int sw=0;
motor_loop(0,loop_cnt);
#if 0
#if 0
int mid=loop_cnt%100;
if(mid<13&&mid>0)
motor_loop(mid,sw);
@@ -277,9 +305,7 @@ void TestMotor::all_motor(){
sw+=1;
motor_loop(1,sw);
}
#endif
#if 0
motor_loop(2,loop_cnt);
motor_loop(3,loop_cnt);
motor_loop(4,loop_cnt);
@@ -291,7 +317,7 @@ void TestMotor::all_motor(){
motor_loop(10,loop_cnt);
motor_loop(11,loop_cnt);
motor_loop(12,loop_cnt);
#endif
#endif
}
int main(int argc,char* argv[]){
rclcpp::init(argc,argv);