IGH Master 主站CSP - Detail-ASD-A2-E

贴出完整代码，适合新手入门学习demo

• cmakeList

cmake_minimum_required(VERSION 3.27)
project(demo)

set(CMAKE_CXX_STANDARD 17)

find_package(EtherCAT REQUIRED)

add_executable(demo main.cpp)

target_link_libraries(demo PUBLIC EtherLab::ethercat)

• demo.cpp

#include <iostream>
#include <algorithm>
#include <csignal>
#include "ecrt.h"

using namespace std;

/****************************************************************************/

/** 任务周期（ns）。 */
#define PERIOD_NS   (1000000) // 1ms

/****************************************************************************/






/****************************************************************************/

/* 常量 */
#define NSEC_PER_SEC (1000000000)

/****************************************************************************/




/****************************************************************************/

// EtherCAT
static ec_master_t *master = nullptr;
static ec_master_state_t master_state = {};

static ec_domain_t *domain1 = nullptr;
static ec_domain_state_t domain1_state = {};

static ec_slave_config_t *sc;
static ec_slave_config_state_t sc_state = {};

/****************************************************************************/






/****************************************************************************/

/*处理数据*/

static uint8_t *domain1_pd = nullptr;

/*总线耦合器位置*/
#define BusCouplerPos  0, 0
/*从站位置*/
#define Detail_ASD_A2_E_1_Pos  0x000001dd,0x10305070 /*Vendor ID, Product code*/

/****************************************************************************/







/****************************************************************************/
/*PDO 条目的偏移量*/

static struct {
    unsigned int operation_mode;
    unsigned int ctrl_word;
    unsigned int target_velocity;
    unsigned int status_word;
    unsigned int current_velocity;
    unsigned int current_position;
} offset;

const static ec_pdo_entry_reg_t domain1_regs[] = {
        {BusCouplerPos, Detail_ASD_A2_E_1_Pos, 0x6040, 0, &offset.ctrl_word},
        {BusCouplerPos, Detail_ASD_A2_E_1_Pos, 0x6041, 0, &offset.status_word},
        {BusCouplerPos, Detail_ASD_A2_E_1_Pos, 0x6064, 0, &offset.current_position},
        {BusCouplerPos, Detail_ASD_A2_E_1_Pos, 0x607A, 0, &offset.target_velocity},
        {}
};

/****************************************************************************/





/****************************************************************************/
/*Config PDOs*/

static ec_pdo_entry_info_t device_pdo_entries[] = {
        /*RxPdo 0x1600*/
        {0x6040, 0x00, 16}, /*Controlword*/
        {0x607A, 0x00, 32}, /*TargetPosition*/
        /*TxPdo 0x1A00*/
        {0x6041, 0x00, 16}, /*Statusword*/
        {0x6064, 0x00, 32}, /*ActualPosition*/
};

static ec_pdo_info_t device_pdos[] = {
        //RxPdo
        {0x1601, 2, device_pdo_entries + 0},
        //TxPdo
        {0x1A01, 2, device_pdo_entries + 2}
};

static ec_sync_info_t device_syncs[] = {
        {0, EC_DIR_OUTPUT, 0, nullptr,         EC_WD_DISABLE},
        {1, EC_DIR_INPUT,  0, nullptr,         EC_WD_DISABLE},
        {2, EC_DIR_OUTPUT, 1, device_pdos + 0, EC_WD_DISABLE},
        {3, EC_DIR_INPUT,  1, device_pdos + 1, EC_WD_DISABLE},
        {0xFF}
};

/****************************************************************************/

void check_domain1_state() {
    ec_domain_state_t ds;
    ecrt_domain_state(domain1, &ds);
    if (ds.working_counter != domain1_state.working_counter) {
        printf("Domain1: WC %u.\n", ds.working_counter);
    }
    if (ds.wc_state != domain1_state.wc_state) {
        printf("Domain1: State %u.\n", ds.wc_state);
    }
    domain1_state = ds;
}

void check_master_state() {
    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() {
    ec_slave_config_state_t s;
    ecrt_slave_config_state(sc, &s);
    if (s.al_state != sc_state.al_state) {
        printf("slave: State 0x%02X.\n", s.al_state);
    }
    if (s.online != sc_state.online) {
        printf("slave: %s.\n", s.online ? "online" : "offline");
    }
    if (s.operational != sc_state.operational) {
        printf("slave: %soperational.\n", s.operational ? "" : "Not ");
    }
    sc_state = s;
}


template<typename type>
void sdoWrite(uint16_t slave, uint16_t index, uint8_t subIndex, const type value) {
    uint32_t abort_code;
    if (ecrt_master_sdo_download(master, slave, index, subIndex, (uint8_t *) &value, sizeof(type), &abort_code))
        cout << "ERROR: ecrt_master_sdo_download: " << abort_code << endl;
}

template<typename type>
type sdoRead(uint16_t slave, uint16_t index, uint8_t subIndex) {
    type value;
    size_t result_size;
    uint32_t abort_code;
    if (ecrt_master_sdo_upload(master, slave, index, subIndex, (uint8_t *) &value, sizeof(type), &result_size,
                               &abort_code) < 0) {
        cout << "ERROR: ecrt_master_sdo_upload: " << abort_code << endl;
        // 如果出现错误，可能需要返回一个默认值或者采取其他适当的错误处理措施
    }
    return value;
}

template<>
// 特化模板
std::string sdoRead<string>(uint16_t slave, uint16_t index, uint8_t subIndex) {
    const size_t maxStringLength = 256;
    uint8_t data[maxStringLength]; // 缓冲区用于存储字符串数据
    size_t resultSize;
    uint32_t abortCode;

    // 读取字符串数据
    if (ecrt_master_sdo_upload(master, slave, index, subIndex, data, maxStringLength, &resultSize, &abortCode) < 0) {
        std::cerr << "ERROR: ecrt_master_sdo_upload: " << abortCode << std::endl;
    }
    // 将字节流转换为字符串
    std::string str(data, data + resultSize);
    // 替换换行符为空字符
    str.erase(std::remove_if(str.begin(), str.end(), [](char c) { return c == '\0'; }), str.end());
    return str;
}

void cyclic_task() {


    /*接收过程数据*/
    ecrt_master_receive(master);
    ecrt_domain_process(domain1);
    /*检查过程数据状态（可选）*/
    check_domain1_state();

    //检查主状态
    check_master_state();
    //检查从站配置状态
    check_slave_config_states();

    /*Read Position*/
    //position = EC_READ_U32(domain1_pd + offset.current_position);
    //fprintf(stderr, "Position: %d\n", position);

    static uint16_t command = 0x004F;//用来帮助判断状态字的值
    int position;
    int status; 

    status = EC_READ_U16(domain1_pd + offset.status_word);
    //EC_WRITE_U16(domain1_pd + offset.ctrl_word, 0x06);
    //EC_WRITE_U16(domain1_pd + offset.ctrl_word, 0x07);
    //EC_WRITE_U16(domain1_pd + offset.ctrl_word, 0x0f);
    // 延时1ms
//    cout << hex << (status & 0x4f) << endl;
    if ((status & 0x4F) == 0x40) {
        EC_WRITE_U16(domain1_pd + offset.ctrl_word, 0x06);
        //printf("Status: 0x40\n");
        command = 0x006F;
    } else if ((status & 0x6F) == 0x21) {
        EC_WRITE_U16(domain1_pd + offset.ctrl_word, 0x07);
        //printf("Status: 0x21\n");
        command = 0x006F;
    } else if ((status & 0x6F) == 0x23) {
        EC_WRITE_U16(domain1_pd + offset.ctrl_word, 0x0f);
        //printf("Status: 0x23\n");
        command = 0x006F;
    } else if((status & command) == 0x27){
        position = EC_READ_S32(domain1_pd + offset.current_position);
        position += 25600;
        EC_WRITE_S32(domain1_pd + offset.target_velocity, position);
    }
    // 发送PDO

    /*发送过程数据*/
    ecrt_domain_queue(domain1);
    ecrt_master_send(master);
}

struct timespec calculate_next_wakeup(const struct timespec &current_time, long period_ms) {
    struct timespec next_wakeup = current_time;
    long sec_to_add = period_ms / 1000;
    long nsec_to_add = (period_ms % 1000) * 1000000;

    next_wakeup.tv_sec += sec_to_add;
    next_wakeup.tv_nsec += nsec_to_add;

    // Adjust tv_sec and tv_nsec if necessary
    if (next_wakeup.tv_nsec >= 1000000000) {
        next_wakeup.tv_sec += 1;
        next_wakeup.tv_nsec -= 1000000000;
    }

    return next_wakeup;
}

int main() {
    printf("Requesting master...\n");
    master = ecrt_request_master(0);
    if (!master) {
        exit(EXIT_FAILURE);
    }

    // 这里可以配置1C12，1C13等配置

    //int slave = 0;

    /* *//*RxPDO*//*
    sdoWrite<int8_t>(slave, 0x1C12, 0, 0);
    *//*TxPDO*//*
    sdoWrite<int8_t>(slave, 0x1C13, 0, 0);

    *//*RxPDO*//*
    sdoWrite<int8_t>(slave, 0x1600, 0, 0);
    sdoWrite<int32_t>(slave, 0x1600, 1, 0x60400010); // Controlword
    sdoWrite<int32_t>(slave, 0x1600, 2, 0x60710010); // Target torque
    sdoWrite<int32_t>(slave, 0x1600, 3, 0x607A0020); // Target position
    sdoWrite<int32_t>(slave, 0x1600, 4, 0x60FF0020); // Target velocity
    sdoWrite<int32_t>(slave, 0x1600, 5, 0x60600008); // Modes of operation display
    sdoWrite<int32_t>(slave, 0x1600, 6, 0x60B20010); // Torque offset
    sdoWrite<int8_t>(slave, 0x1600, 0, 6);

    *//*TxPDO*//*
    sdoWrite<int16_t>(slave, 0x1A00, 0, 0);
    sdoWrite<int32_t>(slave, 0x1A00, 1, 0x60410010); // Statusword
    sdoWrite<int32_t>(slave, 0x1A00, 2, 0x60640020); // Position actual value
    sdoWrite<int32_t>(slave, 0x1A00, 3, 0x606C0020); // Velocity actual value
    sdoWrite<int32_t>(slave, 0x1A00, 4, 0x60770010); // Torque actual value
    sdoWrite<int32_t>(slave, 0x1A00, 5, 0x60610008); // Modes of operation display
    sdoWrite<int8_t>(slave, 0x1A00, 0, 5);

    *//* write 0x1C12 and 0x1C13 (RxPDO and TxPDO) *//*
    sdoWrite<int16_t>(slave, 0x1C12, 1, 0x1600);
    sdoWrite<int8_t>(slave, 0x1C12, 0, 1);

    sdoWrite<int16_t>(slave, 0x1C13, 1, 0x1A00);
    sdoWrite<int8_t>(slave, 0x1C13, 0, 1);*/

    //sdoWrite<int16_t>(0, 0x6040, 0, 0x80);    //reset faluts
    //sdoWrite<int16_t>(0, 0x6040, 7, 0x1);    //reset faluts
    //sdoWrite<int8_t>(0, 0x6060, 0, 8);		//Modes of Operation: 8 [cyclic synchronous position mode]

    //sdoWrite<int8_t>(0, 0x60C2, 1, 4000/100);
    //sdoWrite<int8_t>(0, 0x60C2, 2, -4);

    //string deviceName = sdoRead<string>(slave, 0x1008, 0); // Device name
    //cout << "Device name: " << deviceName << endl;

    //sdoWrite<uint8_t>(0, 0x6060, 0, 8);
    //sdoWrite<uint8_t>(0, 0x60C2, 1, 1);

    domain1 = ecrt_master_create_domain(master);
    if (!domain1) {
        exit(EXIT_FAILURE);
    }
    if (!(sc = ecrt_master_slave_config(master, BusCouplerPos, Detail_ASD_A2_E_1_Pos))) {
        fprintf(stderr, "Failed to get slave configuration for slave!\n");
        exit(EXIT_FAILURE);
    }
    printf("Configuring PDOs...\n");
    if (ecrt_slave_config_pdos(sc, EC_END, device_syncs)) {
        fprintf(stderr, "Failed to configure slave PDOs!\n");
        exit(EXIT_FAILURE);
    } else {
        printf("*Success to configuring slave PDOs*\n");
    }

    if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
        fprintf(stderr, "PDO entry registration failed!\n");
        exit(EXIT_FAILURE);
    }

    ecrt_slave_config_dc(sc, 0x0300, 1000000, 0, 0, 0);
    ecrt_master_select_reference_clock(master, nullptr);
    //ecrt_master_application_time(master, 1);
    //ecrt_master_sync_reference_clock(master);
    //ecrt_master_sync_slave_clocks(master);

    ecrt_slave_config_sdo8(sc, 0x6040, 0, 0x80);

    ecrt_slave_config_sdo8(sc, 0x60C2, 1, 1);
    ecrt_slave_config_sdo8(sc, 0x6060, 0, 8);

    printf("Activating master...\n");
    if (ecrt_master_activate(master)) {
        exit(EXIT_FAILURE);
    } else {
        printf("*Master activated*\n");
    }
    if (!(domain1_pd = ecrt_domain_data(domain1))) {
        exit(EXIT_FAILURE);
    }

    printf("*It's working now*\n");

    struct timespec wakeup_time{};
    clock_gettime(CLOCK_MONOTONIC, &wakeup_time);

    const long PERIOD_MS = 1; // 1ms

    while (true) {
        //usleep(1000);
        //cyclic_task();
        wakeup_time = calculate_next_wakeup(wakeup_time, PERIOD_MS);
        //printf("Next wakeup: %ld.%09ld\n", wakeup_time.tv_sec, wakeup_time.tv_nsec);
        cyclic_task();
        clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &wakeup_time, nullptr);
        //cyclic_task();
    }
    ecrt_release_master(master);
    return EXIT_SUCCESS;
}