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

motor no jitter

Browse Source
This commit is contained in:
Your Name
2026-01-14 20:42:25 +08:00
parent 12365b79e3
commit 3cc5a6a95f
6 changed files with 366 additions and 190 deletions
Download Patch File Download Diff File Expand all files Collapse all files

98
ecrt_dev/config/controllers.yaml
View File

@@ -1,6 +1,6 @@
controller_manager:
ros__parameters:
update_rate: 1000 # Hz
update_rate: 250 # Hz
rt_thread_core: 0
rt_thread_priority: 90
joint_state_broadcaster:
@@ -15,18 +15,18 @@ controller_manager:
trajectory_controller:
ros__parameters:
joints:
- joint_1
- joint_2
- joint_3
- joint_4
- joint_5
- joint_6
- joint_7
- joint_8
- joint_9
- joint_10
- joint_11
- joint_12
- left_arm_joint_1
- left_arm_joint_2
- left_arm_joint_3
- left_arm_joint_4
- left_arm_joint_5
- left_arm_joint_6
- right_arm_joint_1
- right_arm_joint_2
- right_arm_joint_3
- right_arm_joint_4
- right_arm_joint_5
- right_arm_joint_6
command_interfaces:
- position
state_interfaces:
@@ -46,101 +46,101 @@ trajectory_controller:
gpio_command_controller:
ros__parameters:
gpios:
- joint_1
- joint_2
- joint_3
- joint_4
- joint_5
- joint_6
- joint_7
- joint_8
- joint_9
- joint_10
- joint_11
- joint_12
- left_arm_joint_1
- left_arm_joint_2
- left_arm_joint_3
- left_arm_joint_4
- left_arm_joint_5
- left_arm_joint_6
- right_arm_joint_1
- right_arm_joint_2
- right_arm_joint_3
- right_arm_joint_4
- right_arm_joint_5
- right_arm_joint_6
command_interfaces:
joint_1:
left_arm_joint_1:
interfaces:
- fault
- enable
joint_2:
left_arm_joint_2:
interfaces:
- fault
- enable
joint_3:
left_arm_joint_3:
interfaces:
- fault
- enable
joint_4:
left_arm_joint_4:
interfaces:
- fault
- enable
joint_5:
left_arm_joint_5:
interfaces:
- fault
- enable
joint_6:
left_arm_joint_6:
interfaces:
- fault
- enable
joint_7:
right_arm_joint_1:
interfaces:
- fault
- enable
joint_8:
right_arm_joint_2:
interfaces:
- fault
- enable
joint_9:
right_arm_joint_3:
interfaces:
- fault
- enable
joint_10:
right_arm_joint_4:
interfaces:
- fault
- enable
joint_11:
right_arm_joint_5:
interfaces:
- fault
- enable
joint_12:
right_arm_joint_6:
interfaces:
- fault
- enable
state_interfaces:
joint_1:
left_arm_joint_1:
interfaces:
- fault
joint_2:
left_arm_joint_2:
interfaces:
- fault
joint_3:
left_arm_joint_3:
interfaces:
- fault
joint_4:
left_arm_joint_4:
interfaces:
- fault
joint_5:
left_arm_joint_5:
interfaces:
- fault
joint_6:
left_arm_joint_6:
interfaces:
- fault
joint_7:
right_arm_joint_1:
interfaces:
- fault
joint_8:
right_arm_joint_2:
interfaces:
- fault
joint_9:
right_arm_joint_3:
interfaces:
- fault
joint_10:
right_arm_joint_4:
interfaces:
- fault
joint_11:
right_arm_joint_5:
interfaces:
- fault
joint_12:
right_arm_joint_6:
interfaces:
- fault

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

@@ -45,18 +45,18 @@
</hardware>
<!-- 调用子宏生成多个关节（仅需指定名称和位置索引） -->
<xacro:single_joint_config joint_name="joint_1" ec_position="1" />
<xacro:single_joint_config joint_name="joint_2" ec_position="2" />
<xacro:single_joint_config joint_name="joint_3" ec_position="3" />
<xacro:single_joint_config joint_name="joint_4" ec_position="4" />
<xacro:single_joint_config joint_name="joint_5" ec_position="5" />
<xacro:single_joint_config joint_name="joint_6" ec_position="6" />
<xacro:single_joint_config joint_name="joint_7" ec_position="7" />
<xacro:single_joint_config joint_name="joint_8" ec_position="8" />
<xacro:single_joint_config joint_name="joint_9" ec_position="9" />
<xacro:single_joint_config joint_name="joint_10" ec_position="10" />
<xacro:single_joint_config joint_name="joint_11" ec_position="11" />
<xacro:single_joint_config joint_name="joint_12" ec_position="12" />
<xacro:single_joint_config joint_name="left_arm_joint_1" ec_position="1" />
<xacro:single_joint_config joint_name="left_arm_joint_2" ec_position="2" />
<xacro:single_joint_config joint_name="left_arm_joint_3" ec_position="3" />
<xacro:single_joint_config joint_name="left_arm_joint_4" ec_position="4" />
<xacro:single_joint_config joint_name="left_arm_joint_5" ec_position="5" />
<xacro:single_joint_config joint_name="left_arm_joint_6" ec_position="6" />
<xacro:single_joint_config joint_name="right_arm_joint_1" ec_position="7" />
<xacro:single_joint_config joint_name="right_arm_joint_2" ec_position="8" />
<xacro:single_joint_config joint_name="right_arm_joint_3" ec_position="9" />
<xacro:single_joint_config joint_name="right_arm_joint_4" ec_position="10" />
<xacro:single_joint_config joint_name="right_arm_joint_5" ec_position="11" />
<xacro:single_joint_config joint_name="right_arm_joint_6" ec_position="12" />
</ros2_control>
</xacro:macro>

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

@@ -91,6 +91,7 @@ public:
}
struct timespec timespec_add(struct timespec time1, struct timespec time2);
private:
std::chrono::high_resolution_clock::time_point rec_time;
std::array<std::atomic<bool>, NUM_SLAVES> motor_enable_arr;
std::vector<std::unordered_map<std::string, std::string>> getEcModuleParam(
std::string & urdf, std::string component_name, std::string component_type);
@@ -123,13 +124,13 @@ private:
std::mutex ec_mutex_;
///bool activated_;
std::atomic<bool> activated_{false};
#define FREQUENCY 1000
#define CSP_MAX_VEL_COUNTS_PER_S 65536
#define FREQUENCY 250
#define CSP_MAX_VEL_COUNTS_PER_S 25000//65536
#define CSP_POS_DEADBAND 10
#define CSP_DEADBAND1 100 //CSP允许的误差(计数),原来是10
#define CSP_DEADBAND2 500
#define CSP_DEADBAND2 300
#define CSP_SPEED1 30
#define CSP_SPEED2 200
#define CSP_SPEED2 100
#define CLOCK_TO_USE CLOCK_MONOTONIC
#define NSEC_PER_SEC (1000000000L)
#define PERIOD_NS (NSEC_PER_SEC / FREQUENCY) //本次设置周期PERIOD_NS为1ms
@@ -143,6 +144,8 @@ private:
int8_t last_mode_cmd[NUM_SLAVES] = {CYCLIC_VELOCITY};
uint8_t reach_target[NUM_SLAVES];
uint8_t op_states[NUM_SLAVES]={0};
double dst_locs[NUM_SLAVES]={0};
int32_t step_pos[NUM_SLAVES]={0};
int8_t mode_cmd=8;
int inited = 0; //初始化
unsigned int counter = 0;

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

@@ -27,21 +27,6 @@ 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=524288.0/(2*M_PI);//262144.0/(2*M_PI);
constexpr double count_to_speed=2*M_PI/524288.0;//2*M_PI/262144.0;
#define YY_PDO_ENTRY(alias,position,index) \
{alias,position, YY_VID_PID, 0x6040, 0, &zer_offsets[index].ctrl_word,NULL}, \
{alias,position, YY_VID_PID, 0x607A, 0, &zer_offsets[index].target_position,NULL},\
{alias,position, YY_VID_PID, 0x60FF, 0, &zer_offsets[index].target_velocity,NULL},\
{alias,position, YY_VID_PID, 0x6071, 0, &zer_offsets[index].target_torque,NULL},\
{alias,position, YY_VID_PID, 0x6072, 0, &zer_offsets[index].max_torque,NULL},\
{alias,position, YY_VID_PID, 0x6060, 0, &zer_offsets[index].operation_mode,NULL},\
{alias,position, YY_VID_PID, 0xf0ff, 0, &zer_offsets[index].reserved1,NULL},\
{alias,position, YY_VID_PID, 0x6041, 0, &zer_offsets[index].status_word,NULL},\
{alias,position, YY_VID_PID, 0x6064, 0, &zer_offsets[index].position_actual_value,NULL},\
{alias,position, YY_VID_PID, 0x606C, 0, &zer_offsets[index].velocity_actual_value,NULL},\
{alias,position, YY_VID_PID, 0x6077, 0, &zer_offsets[index].torque_actual_value,NULL},\
{alias,position, YY_VID_PID, 0x603F, 0, &zer_offsets[index].error_code,NULL},\
{alias,position, YY_VID_PID, 0x6061, 0, &zer_offsets[index].modes_of_operation_display,NULL},\
{alias,position, YY_VID_PID, 0xf0ff, 0, &zer_offsets[index].reserved2,NULL},
#define PDO_ENTRY(alias,position,index) \
{alias,position, ZER_VID_PID, 0x6040, 0, &zer_offsets[index].ctrl_word,NULL}, \
{alias,position, ZER_VID_PID, 0x607A, 0, &zer_offsets[index].target_position,NULL},\

178
ecrt_dev/src/ethercat_driver.cpp
View File

@@ -1,8 +1,8 @@
#include <sys/mman.h>
#include <pthread.h>
#include <sched.h>
#include <cstring>
#include <unistd.h>
#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>
@@ -22,7 +22,7 @@ CallbackReturn EthercatDriver::on_init(const hardware_interface::HardwareInfo &
cpu_set_t cpuset;
CPU_ZERO(&cpuset);
int cpu_core=3;
int cpu_core=7;
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);
@@ -152,8 +152,9 @@ RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "joints.size:%ld,%ld,%ld",info
perror("ecrt_master_create_domain");
//g_started.store(false); return false;
}
const uint32_t shift_step = PERIOD_NS / 12;
for (int i = 0; i < NUM_SLAVES; i++) {
std::cout << "Configuring slave " << i << "..." << std::endl;
std::cout << "Configuring slave " << i+1 << "..." << std::endl;
{
sc[i] = ecrt_master_slave_config(master, 0, i+1, ZER_VID_PID);
if (!sc[i])
@@ -165,9 +166,14 @@ 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);
//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], 0x0700, PERIOD_NS, 5000, PERIOD_NS/2, 0);
}
///ecrt_master_set_send_interval(master, PERIOD_NS);
ecrt_domain_reg_pdo_entry_list(domain1,zer_domain1_regs);
if (ecrt_master_activate(master)) {
perror("ecrt_master_activate");
@@ -315,6 +321,7 @@ CallbackReturn EthercatDriver::on_activate(
for(int i=0;i<NUM_SLAVES;i++)
RCLCPP_INFO(rclcpp::get_logger("hehe"),"hw_joint_commands_[i][2]:%.1f",hw_joint_commands_[i][2]);
activated_.store(true,std::memory_order_relaxed);
all_motor_op=false;
return CallbackReturn::SUCCESS;
}
@@ -356,8 +363,8 @@ hardware_interface::return_type EthercatDriver::read(
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()>500000)
// RCLCPP_INFO(rclcpp::get_logger("Ethercat"),"diff:%ld",diff.count());
}
return hardware_interface::return_type::OK;
}
@@ -430,20 +437,33 @@ 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
#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));
}
// 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)); // 更新主站应用时间
#endif
ecrt_master_receive(master); //接收 EtherCAT 帧
ecrt_domain_process(domain1); //处理域数据
check_domain1_state(); //检查域状态
@@ -453,8 +473,6 @@ void EthercatDriver::readData(){
counter = FREQUENCY;
// check for master state (optional)
check_master_state(); //检查主站状态
// check for slave configuration state(s)
all_motor_op=check_slave_config_states(); //检查从站状态
if(all_motor_op){
if(!has_clear_all){
has_clear_all=true;
@@ -471,6 +489,7 @@ void EthercatDriver::readData(){
}
}
}else{
all_motor_op=check_slave_config_states(); //检查从站状态
return;
}
}
@@ -482,7 +501,10 @@ 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)
{
@@ -498,8 +520,8 @@ void EthercatDriver::readData(){
hw_joint_states_[i][0]=angle;
hw_joint_states_[i][1]=std::round(vel*count_to_speed*1000.0)/1000.0;
double amp=tv*rated_currents[i];
double effort=7.8*amp-19.19;
hw_joint_states_[i][2]=std::round(amp*10.0)/10.0;
double effort=0.014*0.6*amp;
hw_joint_states_[i][2]=effort;//std::round(amp*10.0)/10.0;
}else{
hw_joint_states_[i][0]=9999;
hw_joint_states_[i][1]=9999;
@@ -507,6 +529,25 @@ 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];
@@ -517,6 +558,9 @@ void EthercatDriver::readData(){
uint16_t err = EC_READ_U16(domain1_pd + zer_offsets[i].error_code);
if(err>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);
}
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);
@@ -530,8 +574,9 @@ 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){
}
@@ -539,54 +584,61 @@ 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);
hw_joint_commands_[i][2]=0;
//if(print_cnt%1000==1)
// RCLCPP_INFO(rclcpp::get_logger("hehe"),"force cmd %d:%.1f",i,hw_joint_commands_[i][2]);
continue;
}
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;
} 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;
} 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;
} else if ((status[i] & command[i]) == 0x0027) {
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 的问题
@@ -602,15 +654,21 @@ 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
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; // 保留余数到下周期
#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) {
@@ -623,34 +681,43 @@ void EthercatDriver::readData(){
#else
const int32_t err_cmd=target_pos-pos;
if (abs(err_cmd)<CSP_DEADBAND1) {
csp_cmd_pos[i] = target_pos;
step_pos[i] = target_pos;
reach_target[i]=1;
}else if(abs(err_cmd)<CSP_DEADBAND2){
if(err_cmd>0)
csp_cmd_pos[i]+=CSP_SPEED1;
step_pos[i]+=CSP_SPEED1;
else
csp_cmd_pos[i]-=CSP_SPEED1;
step_pos[i]-=CSP_SPEED1;
reach_target[i]=0;
} else {
if(reach_target[i]==1){
reach_target[i]=0;
if(err_cmd>0)
csp_cmd_pos[i]+=CSP_SPEED1;
step_pos[i]+=CSP_SPEED1;
else
csp_cmd_pos[i]-=CSP_SPEED1;
step_pos[i]-=CSP_SPEED1;
}else{
if(err_cmd>0)
csp_cmd_pos[i]+=CSP_SPEED2;
step_pos[i]+=CSP_SPEED2;
else
csp_cmd_pos[i]-=CSP_SPEED2;
step_pos[i]-=CSP_SPEED2;
}
}
#endif
#else
csp_cmd_pos[i]=target_pos;
#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]);
//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;
}
@@ -663,6 +730,18 @@ 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 状态
}
}
}
@@ -677,6 +756,13 @@ void EthercatDriver::readData(){
}
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帧

230
ecrt_dev/src/test_motor.cpp
View File

@@ -36,8 +36,13 @@ constexpr double count_to_rad=2*M_PI/524288.0;
bool activateController(const std::string& controller_name);
void setJointValueTarget(double angle);
void setJointValueTarget(const std::vector<double> angles);
void pubTraj(const std::vector<double> angles1,const std::vector<double> angles2);
void pubTraj(const double angle);
bool motor_drv_on=false;
bool all_motor_op=false;
bool is_reach=false;
std::unordered_map<std::string,double> curMap_;
std::unordered_map<std::string,double> dstMap_;
private:
rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr cmdPub_;
rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr gpioPub_;
@@ -54,12 +59,10 @@ constexpr double count_to_rad=2*M_PI/524288.0;
//add by hehe
std::unordered_map<std::string,double> curMap_;
std::unordered_map<std::string,double> dstMap_;
//control_msgs::msg::DynamicInterfaceGroupValues posMsg_;
sensor_msgs::msg::JointState curJointSta_;
int loop_cnt=0;
//add by hehe end
// 执行当前状态对应的动作
void JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg);
@@ -94,13 +97,16 @@ TestMotor::~TestMotor()
{
std::cout << "Robot controller stopped." << std::endl;
}
///std::vector<double> angles={-40, -40, -40, 0, -40, 0,40, 40, 40, 0, 40, 0};
void TestMotor::JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg)
{
if (!msg) { // 检查消息是否有效
std::cout << "get null joint states!" << std::endl;
return;
}
if(msg->name.size()>0)
motor_drv_on=true;
//printf("motor_drv_on:%d\n",motor_drv_on);
all_motor_op=true;
for(int i=0;i<msg->name.size();i++)
{
@@ -109,8 +115,11 @@ void TestMotor::JointStatesCallback(const sensor_msgs::msg::JointState::SharedPt
if(curMap_[msg->name[i]]==9999.0)
all_motor_op=false;
}
if(msg->name.size()>0)
motor_drv_on=true;
if(all_motor_op){
if(is_reach){
////pubTraj(angles);
}
}
}
bool TestMotor::activateController(const std::string& controller_name) {
///std::cout<<"激活控制器"<<controller_name<<std::endl;
@@ -125,29 +134,83 @@ bool TestMotor::activateController(const std::string& controller_name) {
auto result = future.get();
return result->ok;
}
int TestMotor::motor_dst(std::string name,double dst){
control_msgs::msg::DynamicInterfaceGroupValues posMsg_;
double val=curMap_[name];
double diff=dst-val;
double tempJointValue=0;
float delta=0.0;
if(diff>600){
delta=120.0;
tempJointValue=val+delta;
}else if(diff<-600){
delta=-120.0;
tempJointValue=val+delta;
}else{
return 0;
void TestMotor::pubTraj(const std::vector<double> angles1,const std::vector<double> angles2){
trajectory_msgs::msg::JointTrajectory traj_msg;
///traj_msg.header.stamp = this->now();
traj_msg.joint_names = {"left_arm_joint_1", "left_arm_joint_2", "left_arm_joint_3", "left_arm_joint_4", "left_arm_joint_5", "left_arm_joint_6",
"right_arm_joint_1", "right_arm_joint_2", "right_arm_joint_3", "right_arm_joint_4", "right_arm_joint_5", "right_arm_joint_6"};
trajectory_msgs::msg::JointTrajectoryPoint point;
///point.positions = {1.0, 0.5, -0.8, 0.0, 0.7, 0.0}; // 目标角度（弧度）
///point.velocities = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // 到达时速度为0（可选）
/// point.time_from_start = rclcpp::Duration::from_seconds(2.0); // 2秒内到达
double delta=0.0002;
bool skip_pub=true;
for(int i=0;i<6;i++){
std::string jname = "left_arm_joint_" + std::to_string(i+1);
dstMap_[jname]=angles1.at(i)*M_PI/180.0;
double rad=curMap_[jname];
rad=dstMap_[jname];
// double diff=dstMap_[jname]-curMap_[jname];
// if(diff<-0.005){
// rad= curMap_[jname]-delta;
// skip_pub=false;
// }
// else if(diff>0.005){
// rad= curMap_[jname]+delta;
// skip_pub=false;
// }
point.positions.push_back(rad);
printf("D %s: %.2f/%.2f\n",jname.c_str(),curMap_[jname],point.positions[i]);
}
posMsg_.interface_groups.push_back(name);
control_msgs::msg::InterfaceValue tempValue;
tempValue.interface_names = {"position"};
tempValue.values = {tempJointValue};
posMsg_.interface_values.push_back(tempValue);
return 0;
for(int i=0;i<6;i++){
std::string jname = "right_arm_joint_" + std::to_string(i+1);
dstMap_[jname]=angles2.at(i)*M_PI/180.0;
double rad=curMap_[jname];
rad=dstMap_[jname];
point.positions.push_back(rad);
printf("D %s: %.2f/%.2f\n",jname.c_str(),curMap_[jname],point.positions[i]);
}
traj_msg.points.push_back(point);
//if(!skip_pub)
cmdPub_->publish(traj_msg);
}
void TestMotor::pubTraj(const double angle){
trajectory_msgs::msg::JointTrajectory traj_msg;
///traj_msg.header.stamp = this->now();
traj_msg.joint_names = {"left_arm_joint_1", "left_arm_joint_2", "left_arm_joint_3", "left_arm_joint_4", "left_arm_joint_5", "left_arm_joint_6",
"right_arm_joint_1", "right_arm_joint_2", "right_arm_joint_3", "right_arm_joint_4", "right_arm_joint_5", "right_arm_joint_6"};
trajectory_msgs::msg::JointTrajectoryPoint point;
///point.positions = {1.0, 0.5, -0.8, 0.0, 0.7, 0.0}; // 目标角度（弧度）
///point.velocities = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // 到达时速度为0（可选）
/// point.time_from_start = rclcpp::Duration::from_seconds(2.0); // 2秒内到达
double delta=0.0002;
bool skip_pub=true;
for(int i=0;i<6;i++){
std::string jname = "left_arm_joint_" + std::to_string(i+1);
//dstMap_[jname]=angles.at(i)*M_PI/180.0;
//double rad=curMap_[jname];
// double diff=dstMap_[jname]-curMap_[jname];
// if(diff<-0.005){
// rad= curMap_[jname]-delta;
// skip_pub=false;
// }
// else if(diff>0.005){
// rad= curMap_[jname]+delta;
// skip_pub=false;
// }
point.positions.push_back(angle);
printf("S %s: %.2f/%.2f\n",jname.c_str(),curMap_[jname],point.positions[i]);
}
for(int i=0;i<6;i++){
std::string jname = "right_arm_joint_" + std::to_string(i+1);
point.positions.push_back(angle);
printf("S %s: %.2f/%.2f\n",jname.c_str(),curMap_[jname],point.positions[i]);
}
traj_msg.points.push_back(point);
//if(!skip_pub)
cmdPub_->publish(traj_msg);
}
// 状态机主循环
void TestMotor::ControlLoop() {
//Gpio_publish_joint_trajectory();
@@ -158,20 +221,18 @@ void TestMotor::motor_pos(int id,double delta)
control_msgs::msg::DynamicInterfaceGroupValues posMsg_;
std::string target_motor="joint_" + std::to_string(id);
for(int i=0;i<12;i++){
std::string tempInterfaceGroup = "joint_" + std::to_string(i+1);
posMsg_.interface_groups.push_back(tempInterfaceGroup);
std::string jname = "joint_" + std::to_string(i+1);
posMsg_.interface_groups.push_back(jname);
control_msgs::msg::InterfaceValue tempValue;
//position
tempValue.interface_names = {"position"};
if(id==0){
tempValue.values = {curMap_[tempInterfaceGroup]+delta};
}else{
if(tempInterfaceGroup!=target_motor)
tempValue.values = {curMap_[tempInterfaceGroup]};
else
tempValue.values = {curMap_[tempInterfaceGroup]+delta};
}
std::cout<<tempInterfaceGroup<<":"<<curMap_[tempInterfaceGroup]<<"-->"<<tempValue.values[0]<<std::endl;
double diff=dstMap_[jname]-curMap_[jname];
if(diff<-0.03)
tempValue.values = {curMap_[jname]-delta};
else if(diff>0.03)
tempValue.values = {curMap_[jname]+delta};
std::cout<<jname<<":"<<curMap_[jname]<<"-->"<<tempValue.values[0]<<std::endl;
posMsg_.interface_values.push_back(tempValue);
}
@@ -210,27 +271,29 @@ 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);
for(int i=0;i<6;i++){
std::string tempInterfaceGroup = "left_arm_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){
///std::cout<<tempInterfaceGroup<<":"<<tempValue.values[0]<<",";
posMsg_.interface_values.push_back(tempValue);
}
for(int i=0;i<6;i++){
std::string tempInterfaceGroup = "right_arm_joint_" + std::to_string(i+1);
posMsg_.interface_groups.push_back(tempInterfaceGroup);
control_msgs::msg::InterfaceValue tempValue;
//enable
tempValue.interface_names = {"enable"};
if(i<id)
tempValue.values = {enable};
}else{
if(tempInterfaceGroup!=target_motor)
tempValue.values = {0};
else
tempValue.values = {enable};
}
#endif
else
tempValue.values = {0};
///std::cout<<tempInterfaceGroup<<":"<<tempValue.values[0]<<",";
posMsg_.interface_values.push_back(tempValue);
}
@@ -239,16 +302,33 @@ void TestMotor::motor_enable(int id,double enable)
usleep(1000000);
}
void TestMotor::setJointValueTarget(std::vector<double> angles){
return;
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);
// double rad=angles.at(i)*M_PI/180.0;
// point.positions.push_back(rad);
// printf("D %s: %.2f/%.2f\n",joint.c_str(),curMap_[joint],point.positions[i]);
// }
double delta=0.01;
for(int i=0;i<12;i++){
std::string joint="joint_"+std::to_string(i+1);
double rad=angles.at(i)*M_PI/180.0;
std::string jname = "joint_" + std::to_string(i+1);
dstMap_[jname]=angles.at(i)*M_PI/180.0;
double rad=0.0;
double diff=dstMap_[jname]-curMap_[jname];
if(diff<-0.03)
rad= curMap_[jname]-delta;
else if(diff>0.03)
rad= curMap_[jname]+delta;
point.positions.push_back(rad);
printf("D %s: %.2f/%.2f\n",joint.c_str(),curMap_[joint],point.positions[i]);
printf("D %s: %.2f/%.2f\n",jname.c_str(),curMap_[jname],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){
@@ -268,12 +348,14 @@ void TestMotor::setJointValueTarget(std::vector<double> angles){
else if(ret.code==rclcpp_action::ResultCode::ABORTED)
printf("action ret aborted\n");
};
//client_->async_send_goal(goal,send_goal_option);
client_->async_send_goal(goal,send_goal_option);
}else{
printf("wait action server error\n");
}
}
void TestMotor::setJointValueTarget(double angle){
return;
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"};
@@ -309,7 +391,7 @@ void TestMotor::setJointValueTarget(double angle){
}
void TestMotor::motor_action(int id,double delta)
{
//return;
return;
//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();
@@ -395,12 +477,12 @@ int main(int argc,char* argv[]){
rclcpp::spin(node);
});
std::cout<<"等待电机驱动启动...\n";
while(!node->motor_drv_on){
while(rclcpp::ok()&&!node->motor_drv_on){
sleep(1);
}
std::cout<<"电机驱动启动完毕!\n等待电机进入OP...\n";
//std::cout<<""<<std::endl;
while(!node->all_motor_op){
while(rclcpp::ok()&&!node->all_motor_op){
sleep(1);
}
std::cout<<"电机进入OP!\n切换控制器...\n";
@@ -410,21 +492,41 @@ int main(int argc,char* argv[]){
std::cout<<"切换控制器成功!\n准备电机使能...\n";
node->motor_enable(13,1);
std::cout<<"电机使能完毕!\n开始执行角度0...\n";
node->setJointValueTarget(0);
sleep(2);
while(1){
///node->setJointValueTarget(0);
///sleep(10);
std::vector<double> angles1={0, 0, 0, 0, 0, 0};
std::vector<double> angles2={90,0, 40, 0, 0, 0};
node->is_reach=true;
while(rclcpp::ok()){
std::cout<<"开始执行角度1.05...\n";
std::vector<double> angles={-40, -40, -40, 0, -40, 0,40, 40, 40, 0, 0, 40};
node->setJointValueTarget(angles);
//std::vector<double> 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};
// 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;
// printf("dst: %s %.2f/%.2f\n",jname.c_str(),node->curMap_[jname],node->dstMap_[jname]);
// }
///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);
std::cout<<"开始执行复位...\n";
node->setJointValueTarget(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;
// printf("dst: %s %.2f/%.2f\n",jname.c_str(),node->curMap_[jname],node->dstMap_[jname]);
// }
angles1={0, 0, 0, 0, 0, 0};
angles2={0, 0, 0,0, 0, 0};
node->pubTraj(angles1,angles2);
sleep(15);
}
std::cout<<"复位完毕!\n禁能机械臂电机...\n";
node->motor_enable(13,0);
std::cout<<"禁能机械臂电机完毕!\n";
// std::cout<<"复位完毕!\n禁能机械臂电机...\n";
// node->motor_enable(13,0);
// std::cout<<"禁能机械臂电机完毕!\n";
}
///usleep(10000000);
///node->all_motor();