IGH主站通信测试
linuxcnc配置基础机器人控制LinuxCNC与EtherCAT介绍&&PDO&SDO，搭建环境步骤
需要配置IGH主站的查看这篇文章
linux系统学习笔记7——一次性安装igh-ethercat主站
| CSP模式 |
| DC同步方式 |
| preemrt实时补丁 |
直接上代码，这部分是直接控制使用csp模式控制一个从站运动
使能后直接运动，10s，每秒607a（目标位置）增加100.
注意：急停 按下ESC
代码分为两部分，一个是通信线程 主要负责和伺服通信，使能伺服，读取和写入寄存器值。
第二个是操作线程， 负责修改位置的值，和监控按键。
使用此代码，首先根据手册
1.修改PDO条目 ，要和自己的伺服一致
2.修改 PID VID 的值
3.根据自己启动模式，修改6040的控制字，比如笔者的是csp模式，6040：6-7-15 才能使能。
/*********************************************************************
* ethercat通讯驱动禾川伺服驱动器X3EB**********************************
* IGH主站PRMEET RT*************************************************
* HUALAI机械臂******************************************************
* DC同步模式
* csp同步周期位置模式
* 通信成功后运动，急停按下ESC
* 作者：sf9090
* 时间：2023/3/1
* *********************************************************
* ********************************************************************
**********************************************************************/
#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 <sys/mman.h>
#include <pthread.h>
#include <math.h>
#include <malloc.h>
#include <sched.h> /* sched_setscheduler() */
/****************************************************************************/
#include "ecrt.h"
#include <time.h>
/*-------------------------------------监听键盘----------------------------------*/
#include <termios.h>
#include <fcntl.h>
/****************************************************************************/
#include <stdbool.h>
#define FREQUENCY 1000
#define TASK_FREQUENCY 10 /*Hz*/
#define Bool int
#define false 0
#define true 1
#define ETHERCAT_STATUS_OP 0x08
#define STATUS_SERVO_ENABLE_BIT (0x04)
#define CLOCK_TO_USE CLOCK_REALTIME
#define NSEC_PER_SEC (1000000000L)
static int64_t system_time_base = 0LL;
struct timespec wakeupTime;
#define PERIOD_NS (NSEC_PER_SEC / FREQUENCY) /*本次设置周期PERIOD_NS为1ms*/
#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)
//获取当前系统时间
//master status
typedef enum _SysWorkingStatus
{
SYS_WORKING_POWER_ON,
SYS_WORKING_SAFE_MODE,
SYS_WORKING_OP_MODE,
SYS_WORKING_LINK_DOWN,
SYS_WORKING_IDLE_STATUS //系统空闲
}SysWorkingStatus;
typedef struct _GSysRunningParm
{
SysWorkingStatus m_gWorkStatus;
}GSysRunningParm;
GSysRunningParm gSysRunning;
bool Isquit = true;/*判断是否停止*/
bool flag = false;
const int slave = 0;/*从站号*/
int SendNumber = 0;/*发送的次数*/
int ecstate = 0;/*循环次数，用来初始化主站的变量*/
bool IsRun = false;/*判断伺服是否使能*/
/****************************************************************************/
// EtherCAT
ec_master_t *master = NULL;
static ec_master_state_t master_state = {};
static ec_domain_t *domainServoInput = NULL;
static ec_domain_state_t domainServoInput_state = {};
static ec_domain_t *domainServoOutput = NULL;
static ec_domain_state_t domainServoOutput_state = {};
static uint8_t *domainOutput_pd = NULL;
static uint8_t *domainInput_pd = NULL;
static ec_slave_config_t *sc_estun;
static ec_slave_config_state_t sc_estun_state;
/****************************************************************************/
#define estun_Pos0 0, 0
#define estun 0x000116c7, 0x003e0402
// offsets for PDO entries
static unsigned int errcode;
static unsigned int cntlwd;/*控制字*/
static unsigned int ipData;/*目标位置*/
static unsigned int modes_of_operation;/*模式*/
static unsigned int status;/*状态字*/
static unsigned int actpos;/*实际位置*/
static unsigned int modes_of_operation_display;/*读取模式*/
static unsigned int current_velocity;/*当前速度*/
static int cur_status;/*当前状态字*/
static int cur_mode;/*当前模式*/
static int curpos = 0;/*当前位置*/
int curpos_offset = 0;/*设置当前位置*/
// process data
static unsigned int counter = 0;
static unsigned int blink = 0;
static unsigned int sync_ref_counter = 0;
const struct timespec cycletime = {0, PERIOD_NS};
//
ec_pdo_entry_reg_t domainServoOutput_regs[] = {
{estun_Pos0, estun, 0x6040, 0x00, &cntlwd, NULL},
{estun_Pos0, estun, 0x6060, 0x00, &modes_of_operation, NULL}, //6060 模式选择
{estun_Pos0, estun, 0x607A, 0x00, &ipData, NULL},
{}
};
ec_pdo_entry_reg_t domainServoInput_regs[] = {
{estun_Pos0, estun, 0x6041, 0x00, &status, NULL},
{estun_Pos0, estun, 0x606C, 0x00, ¤t_velocity, NULL},
{estun_Pos0, estun, 0x6064, 0x00, &actpos, NULL},
{}
};
/*****************************************************************************/
// 两个时间相加
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;
}
/*********************************************************
* PDO条目根据伺服驱动器说明书上相应的模式设置
* 查看ethercat cstruct
**********************************************************/
ec_pdo_entry_info_t estun_pdo_entries[] = {
/*RxPdo 0x1600*/
{0x6040, 0x00, 16},
{0x6060, 0x00, 8 },
{0x607a, 0x00, 32},
/*TxPdo 0x1A00*/
{0x6041, 0x00, 16},
{0x606C, 0x00, 32},
{0x6064, 0x00, 32}
};
ec_pdo_info_t estun_pdos[] = {
{0x1600, 3, estun_pdo_entries + 0}, /* CoE CSP Mode (RX) */
{0x1a00, 3, estun_pdo_entries + 3}, /* CoE CSP Mode (TX) */
};
ec_sync_info_t estun_syncs[] = {
{0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
{1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
{2, EC_DIR_OUTPUT, 1, estun_pdos + 0, EC_WD_ENABLE},
{3, EC_DIR_INPUT, 1, estun_pdos + 1, EC_WD_DISABLE},
{0xff}
};
/****************************************************************************/
int ConfigPDO()
{
/********************/
printf(" Configuring PDOs...\n");
domainServoOutput = ecrt_master_create_domain(master);
if (!domainServoOutput) {
return -1;
}
domainServoInput = ecrt_master_create_domain(master);
if (!domainServoInput) {
return -1;
}
/********************/
printf(" Creating slave configurations...\n");
sc_estun =
ecrt_master_slave_config(master, estun_Pos0, estun);
if (!sc_estun) {
fprintf(stderr, "Failed to get slave configuration.\n");
return -1;
}
/********************/
if (ecrt_slave_config_pdos(sc_estun, EC_END, estun_syncs)) {
fprintf(stderr, "Failed to configure PDOs.\n");
return -1;
}
/********************/
if (ecrt_domain_reg_pdo_entry_list(domainServoOutput, domainServoOutput_regs)) {
fprintf(stderr, "PDO entry registration failed!\n");
return -1;
}
if (ecrt_domain_reg_pdo_entry_list(domainServoInput, domainServoInput_regs)) {
fprintf(stderr, "PDO entry registration failed!\n");
return -1;
}
fprintf(stderr, "Creating SDO requests...\n");
//有些伺服没有sdo功能的不需要配置
ecrt_slave_config_sdo8(sc_estun, 0x6060, 0, 8);
ecrt_slave_config_sdo8(sc_estun, 0x60C2, 1, 1);
return 0;
}
/*****************************************************************************
* Realtime task
****************************************************************************/
void rt_check_domain_state(void)
{
ec_domain_state_t ds = {};
ec_domain_state_t ds1 = {};
//domainServoInput
ecrt_domain_state(domainServoInput, &ds);
if (ds.working_counter != domainServoInput_state.working_counter) {
printf("domainServoInput: WC %u.\n", ds.working_counter);
}
if (ds.wc_state != domainServoInput_state.wc_state) {
printf("domainServoInput: State %u.\n", ds.wc_state);
}
domainServoInput_state = ds;
//domainServoOutput
ecrt_domain_state(domainServoOutput, &ds1);
if (ds1.working_counter != domainServoOutput_state.working_counter) {
printf("domainServoOutput: WC %u.\n", ds1.working_counter);
}
if (ds1.wc_state != domainServoOutput_state.wc_state) {
printf("domainServoOutput: State %u.\n", ds1.wc_state);
}
domainServoOutput_state = ds1;
}
/****************************************************************************/
void rt_check_master_state(void)
{
ec_master_state_t ms;
ecrt_master_state(master, &ms);
if (ms.slaves_responding != master_state.slaves_responding) {
printf("%u slave(s).\n", ms.slaves_responding);
}
if (ms.al_states != master_state.al_states) {
printf("AL states: 0x%02X.\n", ms.al_states);
}
if (ms.link_up != master_state.link_up) {
printf("Link is %s.\n", ms.link_up ? "up" : "down");
}
master_state = ms;
}
/****************************************************************************/
void check_slave_config_states(void)
{
ec_slave_config_state_t s;
ecrt_slave_config_state(sc_estun,&s);
if (s.al_state != sc_estun_state.al_state)
printf("sc_estun_state: State 0x%02X.\n", s.al_state);
if (s.online != sc_estun_state.online)
printf("sc_estun_state: %s.\n", s.online ? "online" : "offline");
if (s.operational != sc_estun_state.operational)
printf("sc_estun_state: %soperational.\n",s.operational ? "" : "Not ");
sc_estun_state = s;
}
/****************************************************************************/
void ReleaseMaster()
{
if(master)
{
printf(" End of Program, release master\n");
ecrt_release_master(master);
master = NULL;
}
}
/****************************************************************************/
int ActivateMaster()
{
int ret;
printf(" Requesting master...\n");
if(master)
return 0;
master = ecrt_request_master(0);
if (!master) {
return -1;
}
ConfigPDO();
// configure SYNC signals for this slave
ecrt_slave_config_dc(sc_estun, 0x0300, 1000000, 0, 0, 0);
/********************************************************
* 配置从站 sync0 和 sync1 信号
* 最后四个参数表示设置 sync 0和1 的周期和相应的偏移量，单位都是 ns。
* sync0_cycle即为sync0的循环周期，和主栈的周期任务的循环周期保持一致。这个是很重要的一点，需要注意。
* 1、查看 esi 文件，不支持 sync1 同步，所以需要设置成 0x0300
* 0x300: 0x0981的 0,1 位 置 1,其他位 置 0,表示激活运行周期， 激活 sync0
* 0x700: 0x0981的 0,1,2 位 置 1,其他位 置 0,表示激活运行周期， 激活 sync0，和 sync1
* 2、查看 ethercat upload 0x1c32 0x0004 中 bit5-6 为零，代表不支持 shift time，因此第三个参数设置为 0
* 3、一般不使用sync1同步信号，最后两个参数可设置为0。
* 同步周期设置成 1ms
* ********************************************************/
// ecrt_master_application_time(master, system_time_ns());
ret = ecrt_master_select_reference_clock(master, NULL);
if (ret < 0) {
fprintf(stderr, " Failed to select reference clock: %s\n",
strerror(-ret));
return ret;
}
/********************/
printf(" Activating master...\n");
if (ecrt_master_activate(master)) {
printf(" Activating master...failed\n");
return -1;
}
/********************/
if (!(domainInput_pd = ecrt_domain_data(domainServoInput))) {
fprintf(stderr, " Failed to get domain data pointer.\n");
return -1;
}
if (!(domainOutput_pd = ecrt_domain_data(domainServoOutput))) {
fprintf(stderr, " Failed to get domain data pointer.\n");
return -1;
}
printf(" Activating master...success\n");
return 0;
}
/******************************************************************************
* 判断是否有按键按下
******************************************************************************/
int kbhit()
{
struct termios oldt, newt;
int ch;
int oldf;
tcgetattr(STDIN_FILENO, &oldt);
newt = oldt;
newt.c_lflag &= ~(ICANON | ECHO);
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);
ch = getchar();
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
fcntl(STDIN_FILENO, F_SETFL, oldf);
if(ch != EOF)
{
ungetc(ch, stdin);
return 1;
}
return 0;
}
/*******************************************************************************/
/****************************************************************************
* 按键定义
* ESC：退出程序
*
***************************************************************************/
void listen_button()
{
char key;
if(kbhit())//检测是否有键盘输入
{
key=getchar();
printf(" key = %d \n",key);
if(key==27)//esc键的ASCII为27
{
printf("退出程序\n");
curpos_offset = 0;
Isquit = false;
}
}
}
/*********************************************
* 操作线程
*********************************************/
void *OperationThread(void * arg)
{
while(1)
{
usleep(10000);
listen_button();
if(!Isquit){
IsRun = false;
ReleaseMaster();
exit(0);
}
if ( IsRun){//如过伺服驱动器使能，这里就开始运行，知道10s后会停止运动，这里运动是通过修改607a的值实现
if(SendNumber >= 10000)
curpos_offset = 0;
else
curpos_offset=100.0;
}
}
}
/*******************************************************************************/
/****************************************************************************/
/****************************************************************************/
void DriverEtherCAT()
{
static int i = 0;
int status1= 0;
//处于刚开机(需要等待其他操作完成），返回等待下次周期
if(gSysRunning.m_gWorkStatus == SYS_WORKING_POWER_ON)
return ;
static int cycle_counter = 0;
cycle_counter++;
if(cycle_counter >= 120*1000){
cycle_counter = 0;
}
// receive EtherCAT frames
ecrt_master_receive(master);
ecrt_domain_process(domainServoOutput);
ecrt_domain_process(domainServoInput);
rt_check_domain_state();
if (!(cycle_counter % 500)) {
rt_check_master_state();
check_slave_config_states();
}
curpos = EC_READ_S32(domainInput_pd + actpos);
//状态机操作
switch (gSysRunning.m_gWorkStatus)
{
case SYS_WORKING_SAFE_MODE:{
//检查主站是否处于 OP 模式, 若不是，则调整为 OP 模式
rt_check_master_state();
check_slave_config_states();
if((master_state.al_states & ETHERCAT_STATUS_OP))
{
int tmp = true;
if(sc_estun_state.al_state != ETHERCAT_STATUS_OP)
{
tmp = false;
break ;
}
if(tmp)
{
ecstate = 0;
gSysRunning.m_gWorkStatus = SYS_WORKING_OP_MODE;
printf(" SYS_WORKING_OP_MODE\n");
}
}
}break;
case SYS_WORKING_OP_MODE:
{
ecstate++;
//使能伺服
if(ecstate <= 900)
{
switch (ecstate){
case 1:
EC_WRITE_U8(domainOutput_pd + modes_of_operation, 8);
break;
case 200:
// EC_WRITE_U16(domainOutput_pd + cntlwd, 0x80); //错误复位
cur_status= EC_READ_U16(domainOutput_pd+ cntlwd);
printf(" my status_world 0X6040 = %d\n",cur_status);
cur_status = EC_READ_U16(domainInput_pd + status);
printf(" my status_world 0x6041 = %d\n",cur_status);
break;
case 300:
curpos = EC_READ_S32(domainInput_pd + actpos);
EC_WRITE_S32(domainOutput_pd + ipData, curpos);
printf("x@rtITP >>> Axis current position = %d\n", curpos);
break;
case 400:
EC_WRITE_U16(domainOutput_pd + cntlwd, 0x06);
status1= EC_READ_U16(domainInput_pd+ status);
printf(" my status_world 0X6041 = %d \n",status1);
break;
case 500:
EC_WRITE_U16(domainOutput_pd + cntlwd, 0x07);
status1= EC_READ_U16(domainInput_pd+ status);
printf(" my status_world 0X6041 = %d \n",status1);
break;
case 600:
EC_WRITE_U16(domainOutput_pd + cntlwd, 0xF);
status1= EC_READ_U16(domainInput_pd+ status);
printf(" my status_world 0X6041 = %d \n",status1);
break;
}
}
else {
printf("enable servo success!\n");
int tmp = true;
if((EC_READ_U16(domainInput_pd + status) & (STATUS_SERVO_ENABLE_BIT)) == 0)
{
tmp = false;
printf("EC_READ_U16(domainInput_pd + status) & (STATUS_SERVO_ENABLE_BIT)==0\n");
break ;
}
if(tmp)
{
ecstate = 0;
gSysRunning.m_gWorkStatus = SYS_WORKING_IDLE_STATUS;
printf(" SYS_WORKING_IDLE_STATUS\n");
}
}
}break;
default:// 在周期任务里，判断当电机切换为使能状态后，读取电机当前位置，并把这个数值加数据域设置目标作为目标位置写入电机
{
IsRun = true;
EC_WRITE_S32(domainOutput_pd + ipData,curpos+curpos_offset );// 下发位置命令
SendNumber++;
}break;
}
// write application time to master
// ecrt_master_application_time(master, system_time_ns());
ecrt_master_sync_reference_clock(master); //将指定的时间发送给参考时钟，这个函数将最近一次从ecrt_master_application_time传入的时间戳发送给参考时钟。
// 这就是官方example中除了rtai_rtdm_dc这个例子使用的默认dc同步方式，即所谓的以主站作为参考时钟的方式。
ecrt_master_sync_slave_clocks(master); //将DC时钟漂移补偿数据报排队发送，让所有从站时钟与基准时钟同步
// send process data
ecrt_domain_queue(domainServoOutput);
ecrt_domain_queue(domainServoInput);
ecrt_master_send(master);
}
/****************************************************************************/
void *InterpolationThread(void * arg)
{
clock_gettime(CLOCK_TO_USE, &wakeupTime);
while (1) {
usleep(10000);
wakeupTime = timespec_add(wakeupTime, cycletime);
// TIMER_ABSTIME： 代表是采用的绝对时间
// 最后一个参数 rept的意思： 该函数将会使得调用进程处于挂起状态，直到请求的时间到达或者是被信号中断，参数reqtp指定了需要睡眠的秒数与纳秒数。
// 如果在睡眠中途被信号中断，且进程没有终止的话，timespec结构指针remtp指向的结构将保存剩余的睡眠时间，如果我们对于未睡眠时间不感兴趣的话，我们可以把这一时间设置为NULL.
// 如果系统不支持纳秒时间精度的话，请求时间将被向上取整，因为函数nanosleep并不涉及任何信号的产生，我们可以放心大胆地使用它而不用担心与其他函数相互影响。
clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);
ecrt_master_application_time(master, TIMESPEC2NS(wakeupTime));
//Delay the calling task (absolute).Delay the execution of the calling task until a given date is reached.
DriverEtherCAT();
}
}
/****************************************************************************
* Main function
***************************************************************************/
int main(int argc, char *argv[])
{
int ret;
mlockall(MCL_CURRENT | MCL_FUTURE);
gSysRunning.m_gWorkStatus = SYS_WORKING_POWER_ON;
if(gSysRunning.m_gWorkStatus == SYS_WORKING_POWER_ON)
{
ActivateMaster();
ecstate = 0;
gSysRunning.m_gWorkStatus = SYS_WORKING_SAFE_MODE;
printf(" SYS_WORKING_SAFE_MODE\n");
}
printf("Starting InterpolationTask...\n");
pthread_t id[3];
int i = 0;
pthread_create(&id[0],NULL,InterpolationThread,NULL);//通讯线程
pthread_create(&id[1],NULL,OperationThread,NULL);//操作线程
pthread_join(id[0],NULL);
pthread_join(id[1],NULL);
printf(" Deleting realtime InterpolationTask task...\n");
return 0;
}
参考：https://blog.csdn.net/weixin_40293570/article/details/108712655
| 有问题可以在博客下方留言，或者关注微信公众号私信 |
| 【关注微信公众号一起来交流】 |
![Windows 右键菜单扩展容器 [开源]](https://img-blog.csdnimg.cn/img_convert/ac38ccbcadc02ede7033f160708a1f07.png)
