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

del comment

Browse Source
This commit is contained in:
Your Name
2026-01-12 18:09:59 +08:00
parent add2572f4b
commit 12365b79e3
1 changed files with 14 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files

112
ecrt_dev/src/ethercat_driver.cpp
View File

@@ -1,8 +1,8 @@
#include <sys/mman.h> // 用于 mlockall
#include <pthread.h> // 用于线程优先级设置
#include <sched.h> // 用于调度策略
#include <cstring> // 用于 strerror
#include <unistd.h> // 用于 getpid
#include <sys/mman.h>
#include <pthread.h>
#include <sched.h>
#include <cstring>
#include <unistd.h>
#include <sys/resource.h>
#include "ethercat_driver/ethercat_driver.hpp"
#include <tinyxml2.h>
@@ -430,17 +430,7 @@ void EthercatDriver::readData(){
static int print_cnt=0;
print_cnt+=1;
#if 0
if(print_cnt%1000==0){
for(int i=0;i<info_.joints.size();i++){
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++)
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"%.2f,",hw_joint_commands_[i][j]);
//RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"\n");
}
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"\n");
}
#endif
wakeupTime = timespec_add(wakeupTime, cycletime);
////clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL); //使用绝对时间，精确等待下一个周期，避免循环执行的时间造成周期漂移
@@ -492,10 +482,7 @@ void EthercatDriver::readData(){
}
inited = 1;
}
// if(print_cnt%1000==1){
// ////for(int i=0;i<NUM_SLAVES;i++)
// ///RCLCPP_INFO(rclcpp::get_logger("hehe"),"new cmd %d:%.1f",i,hw_joint_commands_[i][2]);
// }
logStream.str("");
for (int i = 0; i < NUM_SLAVES; ++i)
{
@@ -520,25 +507,6 @@ void EthercatDriver::readData(){
}
hw_joint_states_[i][3]=err;
//hw_joint_states_[i][2]=err;
// if(print_cnt%1000==0){
// logStream<<i<<":"<<pos<<"=>"<<up_pos<<","<<angle<<std::endl;
// // if(i==5){
// // std::cout<<"X "<<logStream.str()<<std::endl;
// // logStream.str("");
// // }else if(i==11){
// // std::cout<<"Z "<<logStream.str()<<std::endl;
// // logStream.str("");
// // }
// if(i==11){
// std::cout<<logStream.str()<<std::endl;
// logStream.str("");
// }
// }
//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];
@@ -562,9 +530,8 @@ void EthercatDriver::readData(){
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){
}
@@ -572,29 +539,26 @@ void EthercatDriver::readData(){
}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||op_states[i]!=1)
continue;
double enable=hw_joint_commands_[i][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, 0x0006);
@@ -603,39 +567,26 @@ void EthercatDriver::readData(){
// RCLCPP_INFO(rclcpp::get_logger("hehe"),"force cmd %d:%.1f",i,hw_joint_commands_[i][2]);
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;
//hw_joint_commands_[i][2]=0;//hw_joint_states_[i][0];
} 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);
///hw_joint_commands_[i][2]=0;//hw_joint_states_[i][0];
/////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;
//hw_joint_commands_[i][2]=0;//hw_joint_states_[i][0];
} 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);
break;
case CYCLIC_POSITION: { // 8
// ---- 绝对式目标生成，不再用 pv 做增量基准 ----
// 每轴记住“上次下发的命令位置”
static int32_t csp_cmd_pos[NUM_SLAVES]; // 上次下发到 0x607A 的值
static uint8_t csp_inited[NUM_SLAVES] = {0}; // 是否已初始化
// 定点限速累计器：解决 v_max < FREQUENCY 时整除为 0 的问题
@@ -653,25 +604,9 @@ void EthercatDriver::readData(){
}
double target_pos=down_pos+pos_offsets[i];
//if(print_cnt%1000==0)
// RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "value SET [%d],cur:%d,cmd:%.1f,%.1f,dst:%.1f",i,pos,hw_joint_commands_[i][2],down_pos,target_pos);
if(target_pos>550000)
continue;
// 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;
///
//if(pos_cnt++%500==0)
// RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],cur:%d,dst:%.1f",i,pos,target_pos);
#if 1
vmax_acc[i] += (int64_t)CSP_MAX_VEL_COUNTS_PER_S; // +v [counts/s]
int32_t max_step = (int32_t)(vmax_acc[i] / FREQUENCY); // 下取整，得到本周期可用步长
@@ -711,18 +646,11 @@ void EthercatDriver::readData(){
}
}
#endif
// 可选：跟随误差防护（避免 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 1
// 下发“绝对目标”（不是增量）
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;
}
@@ -735,18 +663,6 @@ void EthercatDriver::readData(){
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 状态
}
}
}