EtherCAT igh主站控制松下伺服（csp模式）

完整代码

#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 <rtdm/rtdm.h>
#include <native/task.h>
#include <native/sem.h>
#include <native/mutex.h>
#include <native/timer.h>
#include <rtdk.h>
#include <pthread.h>
#include <math.h>
#define PI 3.1415926535898

#include "ecrt.h"
#define     Bool                              int
#define     false                             0
#define     true                              1
#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;

RT_TASK 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;
//获取当前系统时间
RTIME system_time_ns(void)
{
    struct timespec  rt_time;
    clock_gettime(CLOCK_TO_USE, &rt_time);
    RTIME 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_estun;
static ec_slave_config_state_t sc_estun_state;
/****************************************************************************/
#define estun_Pos0 0, 0
//#define estun 0x0000060a, 0x00000001
#define estun 0x0000066f, 0x60380006
// 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[] = {
    {estun_Pos0, estun, 0x6040, 0x00, &cntlwd, NULL},
    {estun_Pos0, estun, 0x607a, 0x00, &ipData, NULL},
    {estun_Pos0, estun, 0x6060, 0x00, &modes_of_operation, NULL},			//6060 模式选择
    {}
};
ec_pdo_entry_reg_t domainServoInput_regs[] = {
    {estun_Pos0, estun, 0x6064, 0x00, &actpos, NULL},
    {estun_Pos0, estun, 0x6041, 0x00, &status, NULL},
    {estun_Pos0, estun, 0x6061, 0x00, &modes_of_operation_display, NULL},
    {}
};

/****************************************************************************/
/* Master 0, Slave 0, "MBDLN25BE"
 * Vendor ID:       0x0000066f
 * Product code:    0x60380006
 * Revision number: 0x00010000
 */
ec_pdo_entry_info_t estun_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 estun_pdos[] = {
    {0x1600, 4, estun_pdo_entries + 0}, /* Receive PDO mapping 1 */
    {0x1a00, 8, estun_pdo_entries + 4}, /* Transmit PDO mapping 1 */
};

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("xenomai Configuring PDOs...\n");
    domainServoOutput = ecrt_master_create_domain(master);
    if (!domainServoOutput) {
        return -1;
    }
    domainServoInput = ecrt_master_create_domain(master);
    if (!domainServoInput) {
        return -1;
    }
    /********************/
    printf("xenomai 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");

    // 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) {
        rt_printf("domainServoInput: WC %u.\n", ds.working_counter);
    }
    if (ds.wc_state != domainServoInput_state.wc_state) {
        rt_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) {
        rt_printf("domainServoOutput: WC %u.\n", ds1.working_counter);
    }
    if (ds1.wc_state != domainServoOutput_state.wc_state) {
        rt_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) {
        rt_printf("%u slave(s).\n", ms.slaves_responding);
    }

    if (ms.al_states != master_state.al_states) {
        rt_printf("AL states: 0x%02X.\n", ms.al_states);
    }

    if (ms.link_up != master_state.link_up) {
        rt_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("xenomai End of Program, release master\n");
        ecrt_release_master(master);
        master = NULL;
    }
}
/****************************************************************************/
int ActivateMaster()
{
    int ret;
    printf("xenomai 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);
    // ecrt_slave_config_dc(sc_estun, 0x0300, 4000000, 4000000/2, 0, 0);
    ecrt_master_application_time(master, system_time_ns());
    ret = ecrt_master_select_reference_clock(master, NULL);
    if (ret < 0) {
        fprintf(stderr, "xenomai Failed to select reference clock: %s\n",
                strerror(-ret));
        return ret;
    }

    /********************/
    printf("xenomai Activating master...\n");
    if (ecrt_master_activate(master)) {
        printf("xenomai Activating master...failed\n");
        return -1;
    }
    /********************/
    if (!(domainInput_pd = ecrt_domain_data(domainServoInput))) {
        fprintf(stderr, "xenomai Failed to get domain data pointer.\n");
        return -1;
    }
    if (!(domainOutput_pd = ecrt_domain_data(domainServoOutput))) {
        fprintf(stderr, "xenomai Failed to get domain data pointer.\n");
        return -1;
    }
    printf("xenomai Activating master...success\n");
    return 0;
}
/****************************************************************************/
void DriverEtherCAT()
{
    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++;
    if(cycle_counter >= 600*1000){
        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)) {
        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_estun_state.al_state != ETHERCAT_STATUS_OP)
            {
                tmp = false;
                break ;
            }

            if(tmp)
            {
                ecstate = 0;
                gSysRunning.m_gWorkStatus = SYS_WORKING_OP_MODE;
                printf("xenomai 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);
        printf("curMode: %d\t", cur_mode); //当前操作模式
        cur_status = EC_READ_U16(domainInput_pd + status);
        printf("curStatus: %d\n", cur_status);


        if((cur_status & 0x004f) == 0x0040)
        {
            EC_WRITE_U16(domainOutput_pd + cntlwd, 0x06);
            // cur_mode= EC_READ_U8(domainInput_pd + modes_of_operation_display);
            // printf("curMode: %d\n", cur_mode); //当前操作模式
            printf("0x06\n");
        }
        else if((cur_status & 0x006f) == 0x0021)
        {
            EC_WRITE_U16(domainOutput_pd + cntlwd, 0x07);
            printf("0x07\n");
        }
        else if((cur_status & 0x006f) == 0x023)
        {
            EC_WRITE_U16(domainOutput_pd + cntlwd, 0x0F);
            printf("0x0F\n");
        }
        else if((cur_status & 0x006f) == 0x0027)
        {
            EC_WRITE_U16(domainOutput_pd + cntlwd, 0x001f);
            printf("0x1f\n");

            curpos = EC_READ_S32(domainInput_pd + actpos);    
            // EC_WRITE_S32(domainOutput_pd + ipData, EC_READ_S32(domainInput_pd + actpos)); 
            printf("x@rtITP >>> 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("xenomai SYS_WORKING_IDLE_STATUS\n");
            }
        }


    }break;

    default:
    {
        cur_status = EC_READ_U16(domainInput_pd + status);

	    if (!(cycle_counter % 1000)) {
            printf("curpos = %d\t",curpos);
            printf("actpos... %d\n",EC_READ_S32(domainInput_pd + actpos));
        }
        curpos += 10000;

        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)
{
    RTIME wait, previous;
    previous = rt_timer_read();
    wait = previous;

	while (run) {
        wait += 1000000; //1ms
        //Delay the calling task (absolute).Delay the execution of the calling task until a given date is reached.
        rt_task_sleep_until(wait);
        DriverEtherCAT();
	}
}

/****************************************************************************
 * Signal handler
 ***************************************************************************/

void signal_handler(int sig)
{
    run = 0;
}

/****************************************************************************
 * Main function
 ***************************************************************************/

int main(int argc, char *argv[])
{
    int ret;
    /* Perform auto-init of rt_print buffers if the task doesn't do so */
    rt_print_auto_init(1);
    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("xenomai SYS_WORKING_SAFE_MODE\n"); 
    }

    ret = rt_task_create(&InterpolationTask, "InterpolationTask", 0, 99, T_FPU);
    if (ret < 0) {
        fprintf(stderr, "xenomai Failed to create task: %s\n", strerror(-ret));
        return -1;
    }

    printf("Starting InterpolationTask...\n");
    ret = rt_task_start(&InterpolationTask, &InterpolationThread, NULL);
    if (ret < 0) {
        fprintf(stderr, "xenomai Failed to start task: %s\n", strerror(-ret));
        return -1;
    }

	while (run) {
		rt_task_sleep(50000000);
	}

    printf("xenomai Deleting realtime InterpolationTask task...\n");
    rt_task_delete(&InterpolationTask);

    ReleaseMaster();
    return 0;
}

测试