基于linux环境和ethecat的igh库控制汇川620N的主站代码

基于linux环境和ethecat的igh库控制汇川620N的主站代码

#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 汇川在速度模式下需要在每个任务周期对目标速度进行写操作，否则电机没有反应，这个卡了我好几天才发现）

此文档为 学习笔记
原文链接: http://t.csdnimg.cn/N3ZLK