一、样例描述

本样例是基于EtherCAT  igh 1.6.2版本带载一台高创BD3E伺服，编码器精度为17位，对应电机每转所需指令数为131072个，测试运行代码文件为servoctrl.c，后面描述了如何编译运行。

二、软件环境

1. EtherCAT igh 1.6.2 下载路径：EtherLab / EtherCAT Master · GitLab

2. linux环境：X86 环境 ubuntu20.04 + preempt rt + kernel 5.10

三、代码样例

测试运行代码文件为servoctrl.c

#include <errno.h>
#include <math.h>
#include <pthread.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <sys/mman.h>
#include <sys/resource.h>
#include <sys/time.h>
#include <sys/types.h>
#include <time.h>
#include <unistd.h>

#include "ecrt.h"

#define false 0
#define true 1
#define RUNTIME_MS 60000 // 60s
#define PERIOD_NS 4000000 // 4ms
#define PERIOD_MS (PERIOD_NS / 1000000)
#define ETHERCAT_STATUS_OP 0x08
#define STATUS_SERVO_ENABLE_BIT (0x04)

// 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;

pthread_t InterpolationTask;
int run = 1;
int ecstate = 0;

#define CLOCK_TO_USE CLOCK_REALTIME
#define NSEC_PER_SEC (1000000000L)
#define TIMESPEC2NS(T) ((uint64_t)(T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)

static int64_t system_time_base = 0LL;
// 获取当前系统时间
uint64_t system_time_ns(void)
{
    struct timespec rt_time;
    clock_gettime(CLOCK_TO_USE, &rt_time);
    uint64_t time = TIMESPEC2NS(rt_time);
    return time - system_time_base;
}

/****************************************************************************/
// 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_bd3e;
static ec_slave_config_state_t sc_bd3e_state;
/****************************************************************************/

#define bd3e_Pos0 0, 0
#define bd3e 0x2E1, 0xBE2ED
// offsets for PDO entries
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 Homing_method;
static unsigned int speed_during_search_for_switch;
static unsigned int speed_during_search_for_zero;
static unsigned int homing_acceleration;
static unsigned int home_offset;
static unsigned int cur_status;
static unsigned int cur_mode;

// process data
ec_pdo_entry_reg_t domainServoOutput_regs[] = {
    {bd3e_Pos0, bd3e, 0x6040, 0x00, &cntlwd, NULL},
    {bd3e_Pos0, bd3e, 0x607a, 0x00, &ipData, NULL},
    {bd3e_Pos0, bd3e, 0x6060, 0x00, &modes_of_operation, NULL}, // 6060 模式选择
    {}};
ec_pdo_entry_reg_t domainServoInput_regs[] = {
    {bd3e_Pos0, bd3e, 0x6064, 0x00, &actpos, NULL},
    {bd3e_Pos0, bd3e, 0x6041, 0x00, &status, NULL},
    {bd3e_Pos0, bd3e, 0x6061, 0x00, &modes_of_operation_display, NULL},
    {}};
/****************************************************************************/
/* Master 0, Slave 0, "BD3E"
 * Vendor ID: 0x000002e1
 * Product code: 0xBE2ED
 * Revision number: xxx
 */
ec_pdo_entry_info_t bd3e_pdo_entries[] = {
    {0x6040, 0x00, 16}, /* Controlword */
    {0x6060, 0x00, 8},  /* Modes of operation */
    {0x607a, 0x00, 32}, /* Target position */
    {0x60b8, 0x00, 16}, /* Touch probe function */
    {0x603f, 0x00, 16}, /* Error code */
    {0x6041, 0x00, 16}, /* Statusword */
    {0x6061, 0x00, 8},  /* Modes of operation display */
    {0x6064, 0x00, 32}, /* Position actual value */
    {0x60b9, 0x00, 16}, /* Touch probe status */
    {0x60ba, 0x00, 32}, /* Touch probe pos1 pos value */
    {0x60f4, 0x00, 32}, /* Following error actual value */
    {0x60fd, 0x00, 32}, /* Digital inputs */
};
ec_pdo_info_t bd3e_pdos[] = {
    {0x1600, 4, bd3e_pdo_entries + 0}, /* Receive PDO mapping 1 */
    {0x1a00, 8, bd3e_pdo_entries + 4}, /* Transmit PDO mapping 1 */
};
ec_sync_info_t bd3e_syncs[] = {{0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
                                {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
                                {2, EC_DIR_OUTPUT, 1, bd3e_pdos + 0, EC_WD_ENABLE},
                                {3, EC_DIR_INPUT, 1, bd3e_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_bd3e = ecrt_master_slave_config(master, bd3e_Pos0, bd3e);
    if (!sc_bd3e) {
        fprintf(stderr, "Failed to get slave configuration.\n");
        return -1;
    }
    /********************/
    if (ecrt_slave_config_pdos(sc_bd3e, EC_END, bd3e_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");
    ecrt_slave_config_sdo8(sc_bd3e, 0x6060, 0, 8);
    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_bd3e, &s);
    if (s.al_state != sc_bd3e_state.al_state)
        printf("sc_bd3e_state: State 0x%02X.\n", s.al_state);
    if (s.online != sc_bd3e_state.online)
        printf("sc_bd3e_state: %s.\n", s.online ? "online" : "offline");
    if (s.operational != sc_bd3e_state.operational)
        printf("sc_bd3e_state: %soperational.\n", s.operational ? "" : "Not ");
    sc_bd3e_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_bd3e, 0x0300, PERIOD_NS, 0, 0, 0);
    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;
}
/****************************************************************************/
void DriverEtherCAT()
{
    static int act_encoder = 0;
    static int curpos = 0;
    static int curpos_offset = 0;
    static int i = 0;
    // 处于刚开机(需要等待其他操作完成),返回等待下次周期
    if (gSysRunning.m_gWorkStatus == SYS_WORKING_POWER_ON)
        return;

    static int cycle_counter = 0;
    cycle_counter++;
    int runtime_tick = 0;
    if (PERIOD_MS > 0) {
        runtime_tick = RUNTIME_MS / PERIOD_MS;
    } else {
        runtime_tick = 10000;
    }
    if (cycle_counter >= runtime_tick) {
        cycle_counter = 0;
        run = 0;
    }
    // receive EtherCAT frames
    ecrt_master_receive(master);
    ecrt_domain_process(domainServoOutput);
    ecrt_domain_process(domainServoInput);
    rt_check_domain_state();
    if ((cycle_counter % 500) == 0) {
        rt_check_master_state();
        check_slave_config_states();
    }
    // 状态机操作
    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_bd3e_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");
                EC_WRITE_U16(domainOutput_pd + cntlwd, 0x80); // 错误复位
            }
        }
        break;
    case SYS_WORKING_OP_MODE:
        EC_WRITE_U8(domainOutput_pd + modes_of_operation, 8);
        cur_mode = EC_READ_U8(domainInput_pd + modes_of_operation_display);
        cur_status = EC_READ_U16(domainInput_pd + status);
        if ((cur_status & 0x004f) == 0x0040) {
            EC_WRITE_U16(domainOutput_pd + cntlwd, 0x06);
            cur_mode = EC_READ_U8(domainInput_pd + modes_of_operation_display);
        } else if ((cur_status & 0x006f) == 0x0021) {
            EC_WRITE_U16(domainOutput_pd + cntlwd, 0x07);
        } else if ((cur_status & 0x006f) == 0x023) {
            EC_WRITE_U16(domainOutput_pd + cntlwd, 0x0F);
            curpos = EC_READ_S32(domainInput_pd + actpos);
            EC_WRITE_S32(domainOutput_pd + ipData, EC_READ_S32(domainInput_pd + actpos));
        } else if ((cur_status & 0x006f) == 0x0027) {
            EC_WRITE_U16(domainOutput_pd + cntlwd, 0x001f);
            curpos = EC_READ_S32(domainInput_pd + actpos);
            EC_WRITE_S32(domainOutput_pd + ipData, curpos);
            printf("Axis current position = %d\n", curpos);
            int tmp = true;
            if ((EC_READ_U16(domainInput_pd + status) & (STATUS_SERVO_ENABLE_BIT)) == 0) {
                tmp = false;
                break;
            }
            if (tmp) {
                ecstate = 0;
                gSysRunning.m_gWorkStatus = SYS_WORKING_IDLE_STATUS;
                printf("SYS_WORKING_IDLE_STATUS\n");
            }
        }
        break;
    default:
        cur_status = EC_READ_U16(domainInput_pd + status);
        act_encoder = EC_READ_S32(domainInput_pd + actpos);
        if ((cycle_counter % 250) == 0) {
            printf("curpos = %d, act_encoder=%d\t", curpos, act_encoder);
        }
        // 电机1转所需131072 cmd
        // 1 (转/s) <--> 131072 (cmd/s) <--> 131072/1000 (cmd /ms) <--> 131072 / 1000 * PERIOD_NS / 1000000 (cmd / T)
        // 若需电机每秒1转情况下，则每周期的位置增量为delt = 131072 / 1000 * 4000000 / 1000000 = 524 cmd
        curpos += 524;
        EC_WRITE_S32(domainOutput_pd + ipData, curpos);
        break;
    }
    // write application time to master
    ecrt_master_application_time(master, system_time_ns());
    ecrt_master_sync_reference_clock(master);
    ecrt_master_sync_slave_clocks(master);
    // send process data
    ecrt_domain_queue(domainServoOutput);
    ecrt_domain_queue(domainServoInput);
    ecrt_master_send(master);
}
/****************************************************************************/
void *InterpolationThread(void *arg)
{
    struct timespec wait, previous;
    clock_gettime(CLOCK_REALTIME, &previous);
    wait = previous;
    while (run) {
        wait.tv_nsec += PERIOD_NS; // 4ms
        if (wait.tv_nsec >= NSEC_PER_SEC) {
            wait.tv_sec++;
            wait.tv_nsec -= NSEC_PER_SEC;
        }
        // Delay the calling task (absolute). Delay the execution of the calling task until a given
        // date is reached.
        clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &wait, NULL);
        DriverEtherCAT();
    }
    return NULL;
}
/****************************************************************************
 * Signal handler
 ***************************************************************************/
void signal_handler(int sig)
{
    run = 0;
}
/****************************************************************************
 * Main function
 ***************************************************************************/
int main(int argc, char *argv[])
{
    int ret;
    signal(SIGTERM, signal_handler);
    signal(SIGINT, signal_handler);
    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");
    }
    pthread_attr_t attr;
    struct sched_param param;
    pthread_attr_init(&attr);
    pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
    param.sched_priority = 99;
    pthread_attr_setschedparam(&attr, &param);
    pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
    ret = pthread_create(&InterpolationTask, &attr, InterpolationThread, NULL);
    if (ret) {
        fprintf(stderr, "Failed to create task: %s\n", strerror(ret));
        return -1;
    }
    printf("Starting InterpolationTask...\n");
    while (run) {
        usleep(50000);
    }
    printf("Deleting realtime InterpolationTask task...\n");
    pthread_join(InterpolationTask, NULL);
    ReleaseMaster();
    return 0;
}

代码编译过程如下：

gcc servoctrl.c -o servoctrl -lpthread -lrt -lethercat

其中ethercat lib库为igh编译后的libethercat.so libethercat.so.1，这些库已经编译后拷贝到了/usr/lib，所以可以直接链接lethercat库，否则编译会报错。

代码运行：

sudo ./servoctrl

运行效果如下：

电机表现每秒旋转1圈，运行时间60s。

如果有问题欢迎沟通交流

祝好！