#include "ecrt.h" #include <stdio.h> #include "ethercatdef.h" #include "initInovance.h" #include <errno.h> #include <signal.h> #include <stdio.h> #include <string.h> #include <sys/resource.h> #include <sys/time.h> #include <sys/types.h> #include <unistd.h> #include <time.h> #include <sys/mman.h> #include <malloc.h> static struct timespec timespec_add(struct timespec time1, struct timespec time2); #define CLOCK_TO_USE CLOCK_REALTIME #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) //https://etherlab.org/download/ethercat/ethercat-1.1.1-htmldoc/group__RealtimeInterface.html #define PERIOD_NS (NSEC_PER_SEC / FREQUENCY) #define SHIFT_NS (NSEC_PER_SEC / FREQUENCY /4) static slave_info servo_config [MAX_SLAVES] ={ {POS0_620N,INFO_620N}, {POS1_620N,INFO_620N}, {} }; /* Offsets for PDO entries */ typedef struct { //RxPDO unsigned int control_word; //0x6040:控制字,subindex:0,bitlen:16 unsigned int target_position;//0x607A:目标位置,subindex:0,bitlen:32 unsigned int touch_probe; //0x60B8:探针,subindex:0,bitlen:16 unsigned int pysical_outputs;//0x60FE:pysical_outputs,subindex:1,bitlen:32 unsigned int target_velocity;//0x60FF:target_velocity,subindex:0,bitlen:32 unsigned int target_torque;//0x6071:int target_torque,subindex:0,bitlen:16 unsigned int modes_operation;//0x6060:Modes of operation,subindex:0,bitlen:8 unsigned int max_profile_velocity;//0x607F:max_profile_velocity,subindex:0,bitlen:32 unsigned int positive_torque_limit_value;//0x60E0:positive_torque_limit_value,subindex:0,bitlen:16 unsigned int negative_torque_limit_value;//0x60E1:negaitive_torque_limit_value,subindex:0,bitlen:16 unsigned int torque_offset;//0x60B2:torque offset,subindex:0,bitlen:16 //TxPDo unsigned int status_word;//0x6041:status_word,subindex:0,bitlen:16 unsigned int position_actual_value;//0x6064:position_actual_value,subindex:0,bitlen:32 unsigned int touch_probe_status;//0x60B9,subindex:0,bitlen:16 unsigned int touch_probe_pos1_pos_value;//0x60BA,subindex:0,bitlen:32 unsigned int touch_probe_pos2_pos_value;//0x60BC ,subindex:0,bitlen:32 unsigned int error_code;//0x603F,subindex:0,bitlen:16 unsigned int digital_inputs;//0x60FD,subindex:0,bitlen:32 unsigned int torque_actual_value;//0x6077,subindex:0,bitlen:16 unsigned int following_error_actual_value;//0x60F4,subindex:0,bitlen:32 unsigned int modes_of_operation_display;//0x6061,subindex:0,bitlen:8 unsigned int velocity_actual_value;//0x606C,subindex:0,bitlen:32 //input unsigned int position; }IS620N_offset; IS620N_offset offsetOfIs620n_0; IS620N_offset offsetOfIs620n_1; static IS620N_offset *is620n[MAX_SLAVES]= { &offsetOfIs620n_0, &offsetOfIs620n_1, NULL, }; struct timespec cycletime = {0, PERIOD_NS}; static struct timespec timespec_add(struct timespec time1, struct timespec time2) { struct timespec result; if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC) { result.tv_sec = time1.tv_sec + time2.tv_sec + 1; result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC; } else { result.tv_sec = time1.tv_sec + time2.tv_sec; result.tv_nsec = time1.tv_nsec + time2.tv_nsec; } return result; } int handleTask(IS620N_offset* targetSlave,uint8_t **domain1_pd) { uint16_t state = 0 ,error_code = 0; state = EC_READ_U16(*domain1_pd + targetSlave->status_word); error_code = EC_READ_U16(*domain1_pd + targetSlave->error_code); printf( " status_word: %x \n error_code: %x \n",state,error_code); switch (state){ //伺服初始化状态机 case SERVO_STAT_SWION_DIS: EC_WRITE_U16(*domain1_pd + targetSlave->control_word ,0x0006); break; case SERVO_STAT_RDY_SWION: EC_WRITE_U16(*domain1_pd + targetSlave->control_word ,0x0007); break; case SERVO_STAT_SWION_ENA: EC_WRITE_U16(*domain1_pd + targetSlave->control_word ,0x000f); break; case SERVO_STAT_OP_ENA: EC_WRITE_U8(*domain1_pd + targetSlave->modes_operation ,8); break; case SERVO_STAT_ERROR: EC_WRITE_U16(*domain1_pd + targetSlave->control_word ,0x0080); break; case SERVO_STAT_POSITION_END: if(targetSlave->position>=10000000) return 1; targetSlave->position += 10000; default : if(error_code) { EC_WRITE_U16(*domain1_pd + targetSlave->control_word ,0x0080); printf( " resetting servo\n"); break; } EC_WRITE_U32(*domain1_pd + targetSlave->target_position,targetSlave->position); printf(" offset:%d\n",(targetSlave->position - EC_READ_U32(*domain1_pd + targetSlave->position_actual_value) ) ); break; } return 0; } //每个PERIOD_NS周期进行一次时钟同步,PERIOD_NS*REQUEST_FREQUENCY执行一次任务 void cyclic_task(ec_master_t* master,unsigned slave_cnt ,ec_domain_t* domain1, uint8_t **domain1_pd ) { static unsigned int task_counter = 0; struct timespec wakeupTime, time; clock_gettime(CLOCK_TO_USE, &wakeupTime); unsigned int cnt = 0; int position[MAX_SLAVES]; memset(position,0,sizeof(position)); while(1) { wakeupTime = timespec_add(wakeupTime, cycletime); clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL); /* receive process data */ ecrt_master_receive(master); ecrt_domain_process(domain1); if (task_counter) { task_counter--; } else { /* do this at 1 Hz */ task_counter = REQUEST_FREQUENCY; /* read process data */ for(cnt =0;cnt<slave_cnt;cnt++) { handleTask(is620n[cnt],domain1_pd) ; } } clock_gettime(CLOCK_TO_USE, &time); ecrt_master_application_time(master, TIMESPEC2NS(time)); ecrt_master_sync_reference_clock(master); ecrt_master_sync_slave_clocks(master); ecrt_domain_queue(domain1); ecrt_master_send(master); } } //从站0 的pdo配置 static ec_pdo_entry_reg_t domain0_regs[] = { {POS0_620N,INFO_620N, 0x6040, 0, &offsetOfIs620n_0.control_word}, {POS0_620N,INFO_620N, 0x607A, 0, &offsetOfIs620n_0.target_position}, {POS0_620N,INFO_620N, 0x60FF, 0, &offsetOfIs620n_0.target_velocity}, {POS0_620N,INFO_620N, 0x6071, 0, &offsetOfIs620n_0.target_torque}, {POS0_620N,INFO_620N, 0x6060, 0, &offsetOfIs620n_0.modes_operation}, {POS0_620N,INFO_620N, 0x60B8, 0, &offsetOfIs620n_0.touch_probe}, {POS0_620N,INFO_620N, 0x607F, 0, &offsetOfIs620n_0.max_profile_velocity}, {POS0_620N,INFO_620N, 0x603F, 0, &offsetOfIs620n_0.error_code}, {POS0_620N,INFO_620N, 0x6041, 0, &offsetOfIs620n_0.status_word}, {POS0_620N,INFO_620N, 0x6064, 0, &offsetOfIs620n_0.position_actual_value}, {POS0_620N,INFO_620N, 0x6077, 0, &offsetOfIs620n_0.torque_actual_value}, {POS0_620N,INFO_620N, 0x6061, 0, &offsetOfIs620n_0.touch_probe_status}, {POS0_620N,INFO_620N, 0x60B9, 0, &offsetOfIs620n_0.touch_probe_pos1_pos_value}, {POS0_620N,INFO_620N, 0x60BA, 0, &offsetOfIs620n_0.touch_probe_pos2_pos_value}, {POS0_620N,INFO_620N, 0x60BC, 0, &offsetOfIs620n_0.modes_of_operation_display}, {POS0_620N,INFO_620N, 0x60FD, 0, &offsetOfIs620n_0.digital_inputs}, {} }; //从站1 的pdo配置 static ec_pdo_entry_reg_t domain1_regs[] = { {POS1_620N,INFO_620N, 0x6040, 0, &offsetOfIs620n_1.control_word}, {POS1_620N,INFO_620N, 0x607A, 0, &offsetOfIs620n_1.target_position}, {POS1_620N,INFO_620N, 0x60FF, 0, &offsetOfIs620n_1.target_velocity}, {POS1_620N,INFO_620N, 0x6071, 0, &offsetOfIs620n_1.target_torque}, {POS1_620N,INFO_620N, 0x6060, 0, &offsetOfIs620n_1.modes_operation}, {POS1_620N,INFO_620N, 0x60B8, 0, &offsetOfIs620n_1.touch_probe}, {POS1_620N,INFO_620N, 0x607F, 0, &offsetOfIs620n_1.max_profile_velocity}, {POS1_620N,INFO_620N, 0x603F, 0, &offsetOfIs620n_1.error_code}, {POS1_620N,INFO_620N, 0x6041, 0, &offsetOfIs620n_1.status_word}, {POS1_620N,INFO_620N, 0x6064, 0, &offsetOfIs620n_1.position_actual_value}, {POS1_620N,INFO_620N, 0x6077, 0, &offsetOfIs620n_1.torque_actual_value}, {POS1_620N,INFO_620N, 0x6061, 0, &offsetOfIs620n_1.touch_probe_status}, {POS1_620N,INFO_620N, 0x60B9, 0, &offsetOfIs620n_1.touch_probe_pos1_pos_value}, {POS1_620N,INFO_620N, 0x60BA, 0, &offsetOfIs620n_1.touch_probe_pos2_pos_value}, {POS1_620N,INFO_620N, 0x60BC, 0, &offsetOfIs620n_1.modes_of_operation_display}, {POS1_620N,INFO_620N, 0x60FD, 0, &offsetOfIs620n_1.digital_inputs}, {} }; ec_pdo_entry_reg_t* domain_regs[MAX_SLAVES]={ domain0_regs, domain1_regs, NULL, }; ec_pdo_entry_info_t slave_1_pdo_entries[] = { {0x6040, 0x00, 16}, {0x607A, 0x00, 32}, {0x60FF, 0x00, 32}, {0x6071, 0x00, 16}, {0x6060, 0x00, 8}, {0x60B8, 0x00, 16}, {0x607F, 0x00, 32}, {0x603F, 0x00, 16}, {0x6041, 0x00, 16}, {0x6064, 0x00, 32}, {0x6077, 0x00, 16}, {0x6061, 0x00, 8}, {0x60B9, 0x00, 16}, {0x60BA, 0x00, 32}, {0x60BC, 0x00, 32}, {0x60FD, 0x00, 32} }; ec_pdo_entry_info_t slave_0_pdo_entries[] = { {0x6040, 0x00, 16}, {0x607A, 0x00, 32}, {0x60FF, 0x00, 32}, {0x6071, 0x00, 16}, {0x6060, 0x00, 8}, {0x60B8, 0x00, 16}, {0x607F, 0x00, 32}, {0x603F, 0x00, 16}, {0x6041, 0x00, 16}, {0x6064, 0x00, 32}, {0x6077, 0x00, 16}, {0x6061, 0x00, 8}, {0x60B9, 0x00, 16}, {0x60BA, 0x00, 32}, {0x60BC, 0x00, 32}, {0x60FD, 0x00, 32} }; //注意这里的pdo映射索引 ec_pdo_info_t slave_0_pdos[] = { {0x1702, 7, slave_0_pdo_entries + 0}, {0x1B02 , 9, slave_0_pdo_entries + 7}, }; ec_pdo_info_t slave_1_pdos[] = { {0x1702, 7, slave_1_pdo_entries + 0}, {0x1B02, 9, slave_1_pdo_entries + 7}, }; ec_sync_info_t slave_0_syncs[] = { {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE}, {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE}, {2, EC_DIR_OUTPUT, 1, slave_0_pdos + 0, EC_WD_ENABLE}, {3, EC_DIR_INPUT, 1, slave_0_pdos + 1, EC_WD_DISABLE}, {0xff} }; ec_sync_info_t slave_1_syncs[] = { {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE}, {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE}, {2, EC_DIR_OUTPUT, 1, slave_1_pdos + 0, EC_WD_ENABLE}, {3, EC_DIR_INPUT, 1, slave_1_pdos + 1, EC_WD_DISABLE}, {0xff} }; ec_sync_info_t* slave_syncs[MAX_SLAVES]={ slave_0_syncs, slave_1_syncs, NULL, }; //sdo配置,在激活master之前可以调用 int configParamOfServo(ec_master_t *master, slave_info *servo_config,unsigned int slave_cnt) { uint16_t encoder_config = 14101; uint32_t celeration = 200; uint32_t abort_code = 0; uint32_t deceleration = 200; uint32_t maxVelocity = 0x0a6aaaaa; uint16_t velocity_loop_gain = 401; int cnt = 0; for(cnt=0;cnt<slave_cnt;cnt++) { if( 0 > ecrt_master_sdo_download(master,servo_config[cnt].position,0x6083,0,(uint8_t*)&celeration,4,&abort_code))goto error_out; if( 0 > ecrt_master_sdo_download(master,servo_config[cnt].position,0x6084,0,(uint8_t*)&deceleration,4,&abort_code))goto error_out; if( 0 > ecrt_master_sdo_download(master,servo_config[cnt].position,0x2000,1,(uint8_t*)&encoder_config,2,&abort_code))goto error_out; if( 0 > ecrt_master_sdo_download(master,servo_config[cnt].position,0x607f,0,(uint8_t*)&maxVelocity,4,&abort_code))goto error_out; if( 0 > ecrt_master_sdo_download(master,servo_config[cnt].position,0x2008,1,(uint8_t*)&velocity_loop_gain,2,&abort_code))goto error_out; } return 0; error_out: printf("error!!,sdo config failed abort code %x\n",abort_code); return 1; } int main(void) { ec_master_t * master =NULL; ec_domain_t* domain = NULL; uint8_t * domain_pd = NULL; /* process data */ ec_slave_config_t* slave[MAX_SLAVES]; ec_master_state_t state; memset(slave,0,sizeof(slave)); master = ecrt_request_master( 0 ); if(master == NULL) goto error_out; printf("master found\n"); ecrt_master_state( master, &state ); //获取master信息 domain = ecrt_master_create_domain(master); if (!domain) goto error_out; int cnt; for(cnt =0;cnt<state.slaves_responding;cnt++) { slave[cnt] = ecrt_master_slave_config(master,servo_config[cnt].alias,\ servo_config[cnt].position,\ servo_config[cnt].vendor_id,\ servo_config[cnt].product_code); if (!slave[cnt]) goto error_out; } printf("Configuring SDOs...\n"); if(0!=configParamOfServo(master, servo_config,state.slaves_responding))goto error_out; printf("Configuring PDOs...\n"); for(cnt =0;cnt<state.slaves_responding;cnt++) { if (ecrt_slave_config_pdos(slave[cnt], EC_END, slave_syncs[cnt])) goto error_out; } for(cnt =0;cnt<state.slaves_responding;cnt++) { if (ecrt_domain_reg_pdo_entry_list(domain, domain_regs[cnt])) goto error_out; } for(cnt =0;cnt<state.slaves_responding;cnt++) { ecrt_slave_config_dc(slave[cnt], 0x300, PERIOD_NS, SHIFT_NS, 0, 0) ; } printf("Activating master...\n"); if ( ecrt_master_activate(master) ) goto error_out; if (!(domain_pd = ecrt_domain_data(domain))) goto error_out; printf("Start !\n"); cyclic_task(master,state.slaves_responding,domain,&(domain_pd)); return 0; error_out: printf("error!!start failed\n"); return 0 ; }
关于获取master的信息来获得slave个数 参考了 https://etherlab-users.etherlab.narkive.com/IkBhZEoF/how-to-get-slave-count-from-userspace 的方法(我只会down代码而已,谈不上技术程序员)
汇川伺服手册需要参考,对不同的伺服都需要一本手册在手,了解pdo映射条目,sdo索引的具体含义。
我选择的是同步位置模式,控制两个电机转一定的位置
(ps 汇川在速度模式下需要在每个任务周期对目标速度进行写操作,否则电机没有反应,这个卡了我好几天才发现)
原文链接:https://blog.csdn.net/weixin_42186805/article/details/108603423