From c6deefe96d9c5ee93f3f3570d51b647376e2576e Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 21 Jan 2026 10:57:14 +0800 Subject: [PATCH] split read write& solve data error --- ecrt_dev/config/controllers.yaml | 12 + .../motor_drive.ros2_control.xacro | 1 + .../ethercat_driver/ethercat_driver.hpp | 10 +- ecrt_dev/launch/motor_drive.launch.py | 3 +- ecrt_dev/src/ethercat_driver.cpp | 210 +++++++----------- ecrt_dev/src/test_motor.cpp | 28 ++- 6 files changed, 115 insertions(+), 149 deletions(-) diff --git a/ecrt_dev/config/controllers.yaml b/ecrt_dev/config/controllers.yaml index c9a3a78..44ee106 100644 --- a/ecrt_dev/config/controllers.yaml +++ b/ecrt_dev/config/controllers.yaml @@ -111,36 +111,48 @@ gpio_command_controller: left_arm_joint_1: interfaces: - fault + - enable left_arm_joint_2: interfaces: - fault + - enable left_arm_joint_3: interfaces: - fault + - enable left_arm_joint_4: interfaces: - fault + - enable left_arm_joint_5: interfaces: - fault + - enable left_arm_joint_6: interfaces: - fault + - enable right_arm_joint_1: interfaces: - fault + - enable right_arm_joint_2: interfaces: - fault + - enable right_arm_joint_3: interfaces: - fault + - enable right_arm_joint_4: interfaces: - fault + - enable right_arm_joint_5: interfaces: - fault + - enable right_arm_joint_6: interfaces: - fault + - enable diff --git a/ecrt_dev/description/motor_drive.ros2_control.xacro b/ecrt_dev/description/motor_drive.ros2_control.xacro index 35a177f..2718313 100644 --- a/ecrt_dev/description/motor_drive.ros2_control.xacro +++ b/ecrt_dev/description/motor_drive.ros2_control.xacro @@ -18,6 +18,7 @@ + diff --git a/ecrt_dev/include/ethercat_driver/ethercat_driver.hpp b/ecrt_dev/include/ethercat_driver/ethercat_driver.hpp index 5f1d056..3c95dd2 100644 --- a/ecrt_dev/include/ethercat_driver/ethercat_driver.hpp +++ b/ecrt_dev/include/ethercat_driver/ethercat_driver.hpp @@ -125,11 +125,11 @@ private: ///bool activated_; std::atomic activated_{false}; #define FREQUENCY 250 - #define CSP_MAX_VEL_COUNTS_PER_S 25000//65536 + #define CSP_MAX_VEL_COUNTS_PER_S 65536 #define CSP_POS_DEADBAND 10 - #define CSP_DEADBAND1 100 //CSP允许的误差(计数),原来是10 - #define CSP_DEADBAND2 300 - #define CSP_SPEED1 30 + #define CSP_DEADBAND1 100//100 //CSP允许的误差(计数),原来是10 + #define CSP_DEADBAND2 1000 //300 + #define CSP_SPEED1 20//30 #define CSP_SPEED2 100 #define CLOCK_TO_USE CLOCK_MONOTONIC #define NSEC_PER_SEC (1000000000L) @@ -146,6 +146,8 @@ private: uint8_t op_states[NUM_SLAVES]={0}; double dst_locs[NUM_SLAVES]={0}; int32_t step_pos[NUM_SLAVES]={0}; + int32_t cur_pos[NUM_SLAVES]={0}; + double enable_arr[NUM_SLAVES]={0}; int8_t mode_cmd=8; int inited = 0; //初始化 unsigned int counter = 0; diff --git a/ecrt_dev/launch/motor_drive.launch.py b/ecrt_dev/launch/motor_drive.launch.py index bd73d28..4603024 100644 --- a/ecrt_dev/launch/motor_drive.launch.py +++ b/ecrt_dev/launch/motor_drive.launch.py @@ -52,7 +52,8 @@ def generate_launch_description(): parameters=[robot_description, robot_controllers, { "lock_memory": True, - "thread_priority": 90 + "thread_priority": 99, + "cpu_affinity": 6 } ], output="both", diff --git a/ecrt_dev/src/ethercat_driver.cpp b/ecrt_dev/src/ethercat_driver.cpp index ecac416..f3a7dd2 100644 --- a/ecrt_dev/src/ethercat_driver.cpp +++ b/ecrt_dev/src/ethercat_driver.cpp @@ -30,7 +30,7 @@ CallbackReturn EthercatDriver::on_init(const hardware_interface::HardwareInfo & } struct sched_param param; - param.sched_priority = 99; + param.sched_priority = 90; if (pthread_setschedparam(pthread_self(), SCHED_FIFO, ¶m) != 0) { RCLCPP_ERROR(rclcpp::get_logger("EthercatDriver"), @@ -166,11 +166,7 @@ RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "joints.size:%ld,%ld,%ld",info std::cout << "Failed PDO config " << i << std::endl; } } - //ecrt_slave_config_dc(sc[i], 0x0300, PERIOD_NS, 5000, 0, 0); - if(i==8) - ecrt_slave_config_dc(sc[i], 0x0300, PERIOD_NS, 5000,2500, 0); - else - ecrt_slave_config_dc(sc[i], 0x0300, PERIOD_NS, 5000,0, 0); + ecrt_slave_config_dc(sc[i], 0x0300, PERIOD_NS, 20000, 0, 0); //ecrt_slave_config_dc(sc[i], 0x0700, PERIOD_NS, 5000, PERIOD_NS/2, 0); } ///ecrt_master_set_send_interval(master, PERIOD_NS); @@ -205,7 +201,7 @@ EthercatDriver::export_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",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, @@ -255,7 +251,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",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, @@ -358,13 +354,12 @@ hardware_interface::return_type EthercatDriver::read( //printf("read data...\n"); //if (lock.owns_lock() && activated_) { if (activated_.load(std::memory_order_relaxed)){ - ////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()); + if(diff.count()>100000) + RCLCPP_INFO(rclcpp::get_logger("Ethercat"),"rdiff:%ld",diff.count()); } return hardware_interface::return_type::OK; } @@ -431,8 +426,6 @@ struct timespec EthercatDriver::timespec_add(struct timespec time1, struct times return result; } -#include -static std::stringstream logStream; void EthercatDriver::readData(){ static int print_cnt=0; @@ -450,29 +443,17 @@ void EthercatDriver::readData(){ #endif #if 1 wakeupTime = timespec_add(wakeupTime, cycletime); - ////clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL); //使用绝对时间，精确等待下一个周期，避免循环执行的时间造成周期漂移 - - // int ret = clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL); - // if (ret != 0) { - // // 睡眠失败（如wakeupTime落后于当前时间），重置唤醒时间 - // clock_gettime(CLOCK_TO_USE, &wakeupTime); // 获取当前时间 - // wakeupTime = timespec_add(wakeupTime, cycletime); // 重新计算下一个周期 - // RCLCPP_WARN(rclcpp::get_logger("EthercatDriver"), - // "clock_nanosleep failed: %s, reset wakeupTime", strerror(ret)); - // } - ecrt_master_application_time(master, TIMESPEC2NS(wakeupTime)); // 更新主站应用时间 + ////clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL); + ecrt_master_application_time(master, TIMESPEC2NS(wakeupTime)); #endif - - - ecrt_master_receive(master); //接收 EtherCAT 帧 - ecrt_domain_process(domain1); //处理域数据 - check_domain1_state(); //检查域状态 + ecrt_master_receive(master); + ecrt_domain_process(domain1); + 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(); if(all_motor_op){ if(!has_clear_all){ has_clear_all=true; @@ -501,11 +482,7 @@ void EthercatDriver::readData(){ } inited = 1; } - // if(print_cnt%1000==1){ - // ////for(int i=0;i"<0){ double fault=hw_joint_commands_[i][0]; if(print_cnt%1000==1){ - RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],EE:0x%04x",i,err); + ///RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],EE:0x%04x",i,err); } if(fault==1.0||clear_cmd[i]==1){ if(print_cnt%1000==0) @@ -594,7 +564,7 @@ void EthercatDriver::readData(){ 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){ + if((status[i]&0x006f)==0x0008){ errStaArr[i]=ERR_ACK; }else{ errStaArr[i]=ERR_NONE; @@ -604,10 +574,10 @@ void EthercatDriver::readData(){ #endif if(errStaArr[i]!=ERR_NONE||op_states[i]!=1) continue; - double enable=hw_joint_commands_[i][1]; + enable_arr[i]=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){ + if(enable_arr[i]!=1.0){ EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, 0); EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0006); hw_joint_commands_[i][2]=0; @@ -616,18 +586,17 @@ void EthercatDriver::readData(){ 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; - step_pos[i]=pos; + step_pos[i]=cur_pos[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); - step_pos[i]=pos; + step_pos[i]=cur_pos[i]; } else if ((status[i] & command[i]) == 0x0023) { EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x000f); // Operation enabled command[i] = 0x006F; - step_pos[i]=pos; + step_pos[i]=cur_pos[i]; } else if ((status[i] & command[i]) == 0x0027) { if(true) { @@ -646,7 +615,7 @@ void EthercatDriver::readData(){ // 进入/首次运行：把命令位置对齐到“当前实际位置”，避免冲击 if (!csp_inited[i] || last_mode_cmd[i] != CYCLIC_POSITION) { - csp_cmd_pos[i] = pos; // 先对齐 + csp_cmd_pos[i] = cur_pos[i]; // 先对齐 vmax_acc[i] = 0; csp_inited[i] = 1; } @@ -654,59 +623,43 @@ void EthercatDriver::readData(){ if(i==2||i==8){ down_pos=-down_pos; } - double target_pos=down_pos+pos_offsets[i]; if(print_cnt%1000==1){ //if(i==8) // RCLCPP_INFO(rclcpp::get_logger("hehe"),"hw_joint_commands_[i][2]:%.3f,target_pos:%.3f",hw_joint_commands_[i][2],target_pos); } - if(target_pos>550000) - continue; dst_locs[i]=target_pos; -#if 1 -#if 0 - ///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; // 保留余数到下周期 - int32_t max_step=CSP_MAX_VEL_COUNTS_PER_S/FREQUENCY; - const int32_t goal=target_pos; - const int32_t err_cmd = goal -csp_cmd_pos[i]; - 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; - } -#else - const int32_t err_cmd=target_pos-pos; - if (abs(err_cmd)0) - step_pos[i]+=CSP_SPEED1; - else - step_pos[i]-=CSP_SPEED1; - reach_target[i]=0; - } else { - if(reach_target[i]==1){ - reach_target[i]=0; - if(err_cmd>0) - step_pos[i]+=CSP_SPEED1; - else - step_pos[i]-=CSP_SPEED1; - }else{ - if(err_cmd>0) - step_pos[i]+=CSP_SPEED2; - else - step_pos[i]-=CSP_SPEED2; - } - } -#endif -#else - csp_cmd_pos[i]=target_pos; -#endif + #if 0 + int32_t max_step=CSP_MAX_VEL_COUNTS_PER_S/FREQUENCY; + const int32_t goal=target_pos; + const int32_t err_cmd = goal -csp_cmd_pos[i]; + 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; + } + EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, csp_cmd_pos[i]); + #else + const int32_t err_cmd=target_pos-cur_pos[i]; + if (abs(err_cmd)0) step_pos[i]+=(err_cmd>CSP_SPEED1) ? CSP_SPEED1:err_cmd; + else step_pos[i]+=(err_cmd<-CSP_SPEED1) ? -CSP_SPEED1:err_cmd; + // reach_target[i]=0; + //if(err_cmd>0) step_pos[i]+=CSP_SPEED1; + //else step_pos[i]-=CSP_SPEED1; + } else { + //if(err_cmd>0) step_pos[i]+=(err_cmd>CSP_SPEED2) ? CSP_SPEED2:err_cmd; + //else step_pos[i]+=(err_cmd<-CSP_SPEED2) ? -CSP_SPEED2:err_cmd; + if(err_cmd>0) step_pos[i]+=CSP_SPEED2; + else step_pos[i]-=CSP_SPEED2; + } + EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, step_pos[i]); + #endif // 可选：跟随误差防护（避免 following error 过大时继续“加命令”） // const int32_t follow_err = csp_cmd_pos[i] - pv; // const int32_t FE_LIMIT = (int32_t) (你的驱动跟随误差阈值); // 参考驱动参数 @@ -714,11 +667,6 @@ void EthercatDriver::readData(){ // // 例如：冻结或减半 max_step，给驱动时间追上 // } - #if 1 - // 下发“绝对目标”（不是增量） - //EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, csp_cmd_pos[i]); - EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, step_pos[i]); - #endif break; } case CYCLIC_TORQUE: // 10 @@ -750,20 +698,11 @@ void EthercatDriver::readData(){ 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); } - ecrt_master_sync_slave_clocks(master); -#if 0 - auto diff=std::chrono::high_resolution_clock::now()-rec_time; - rec_time=std::chrono::high_resolution_clock::now(); - long ttv=diff.count()-4000000; - if(abs(ttv)>100000) - RCLCPP_INFO(rclcpp::get_logger("Ethercat"),"exe :%ld,%ld",diff.count(),ttv); -#endif - // send process data ecrt_domain_queue(domain1); //排队域数据 ecrt_master_send(master); //发送ETHERCAT帧 } @@ -773,10 +712,13 @@ hardware_interface::return_type EthercatDriver::write( { // try to lock so we can avoid blocking the read/write loop on the lock. /// const std::unique_lock lock(ec_mutex_, std::try_to_lock); - //printf("write data...\n"); - ///if (lock.owns_lock() && activated_) - { - //writeData(); + if (activated_.load(std::memory_order_relaxed)){ + auto start=std::chrono::high_resolution_clock::now(); + writeData(); + auto end=std::chrono::high_resolution_clock::now(); + auto diff=end-start; + if(diff.count()>100000) + RCLCPP_INFO(rclcpp::get_logger("Ethercat"),"wdiff:%ld",diff.count()); } return hardware_interface::return_type::OK; } diff --git a/ecrt_dev/src/test_motor.cpp b/ecrt_dev/src/test_motor.cpp index 15005c7..5209b4b 100644 --- a/ecrt_dev/src/test_motor.cpp +++ b/ecrt_dev/src/test_motor.cpp @@ -160,7 +160,7 @@ void TestMotor::pubTraj(const std::vector angles1,const std::vector angles1,const std::vector angles1={0, 0, 0, 0, 0, 0}; std::vector angles2={90,0, 40, 0, 0, 0}; node->is_reach=true; + static int loop_cnt=0; while(rclcpp::ok()){ std::cout<<"开始执行角度1.05...\n"; - //std::vector angles={-40, -40, -40, 0, -40, 0,40, 40, 40, 0, 40, 0}; - //angles={0, 0, 0, 0, 0, 0,40,0, 0, 0, 0, 00}; - angles1={0, 0, 0, 0, 0, 0}; - angles2={0, 0, 40,40, 0, 0}; + loop_cnt+=1; + if(loop_cnt%2==0){ + angles1={-40, -40, -40,-40,-40,-40}; + angles2={-40,-40,-40, -40,-40,-40}; + }else{ + angles1={40, 40, 40,40,40,40}; + angles2={40,40,40, 40,40,40}; + } + //angles2={0,0,0, 40,0,0}; + //angles1={-40,-40,-40, 0,-40, 0}; + //angles2={40,40,40, 0,40,0}; // for(int i=0;i<12;i++){ // std::string jname = "joint_" + std::to_string(i+1); // node->dstMap_[jname]=angles.at(i)*M_PI/180.0; @@ -511,7 +519,7 @@ int main(int argc,char* argv[]){ // } ///node->dstMap_[jname]={-0.678,-0.678,-0.678,0,-0.678,0,0.678,0.678,0.678,0,0.678,0}; node->pubTraj(angles1,angles2); - sleep(15); + sleep(10); std::cout<<"开始执行复位...\n"; // for(int i=0;i<12;i++){ // std::string jname = "joint_" + std::to_string(i+1); @@ -521,7 +529,7 @@ int main(int argc,char* argv[]){ angles1={0, 0, 0, 0, 0, 0}; angles2={0, 0, 0,0, 0, 0}; node->pubTraj(angles1,angles2); - sleep(15); + sleep(10); } // std::cout<<"复位完毕!\n禁能机械臂电机...\n";