igh ethercat雷赛dm3c 示例程序

以下是一个基于EtherCAT总线的雷赛DM3C示例程序，使用C++语言编写：

#include <iostream>
#include <string>
#include <vector>
#include <ecrt.h>

#define VENDOR_ID       0x1c11
#define PRODUCT_CODE    0x0003

#define DOMAIN_IDX      0
#define DRIVE_IDX       1

#define CONTROLWORD     0x6040
#define STATUSWORD      0x6041
#define TARGET_POS      0x607a
#define TARGET_VEL      0x60ff
#define PROFILE_VEL     0x6081
#define PROFILE_ACC     0x6083
#define PROFILE_DEC     0x6084
#define MODES_OF_OPE    0x6060

#define STATE_INIT              0
#define STATE_PRE_OP            1
#define STATE_SAFE_OP           2
#define STATE_OP                3
#define STATE_UNKNOWN           4

typedef struct {
    uint8_t *domain_memory;
    ec_domain_t *domain;
    ec_slave_config_t *drive_config;
    uint16_t controlword;
    uint16_t statusword;
    int32_t target_pos;
    int32_t target_vel;
    int32_t profile_vel;
    int32_t profile_acc;
    int32_t profile_dec;
    uint8_t modes_of_ope;
    uint8_t state;
} drive_t;

static ec_master_t *master = NULL;
static ec_master_info_t master_info;
static std::vector<drive_t> drives;

static uint8_t *domain_memory = NULL;
static ec_domain_t *domain = NULL;

static void check_domain_state(void);

int main(int argc, char **argv) {
    int ret = 0;
    uint32_t domain_size;
    ec_slave_config_t *drive_config;
    drive_t drive;
    uint16_t product_code, vendor_id;
    ec_slave_info_t slave_info;

    /* Initialize EtherCAT master */
    if (ecrt_master_open(&master_info) != 0) {
        std::cerr << "Failed to open EtherCAT master." << std::endl;
        return -1;
    }

    master = ecrt_master_create();
    if (master == NULL) {
        std::cerr << "Failed to create EtherCAT master." << std::endl;
        return -1;
    }

    ecrt_master_set_slave_config_callback(master, ecrt_slave_config_dc);
    if (ecrt_master_set_slave_config(master, NULL) != 0) {
        std::cerr << "Failed to set slave configuration." << std::endl;
        return -1;
    }

    if (ecrt_master_activate(master) != 0) {
        std::cerr << "Failed to activate EtherCAT master." << std::endl;
        return -1;
    }

    /* Get domain size */
    domain_size = ecrt_master_get_expected_master_size(master);
    domain_memory = (uint8_t*)malloc(domain_size);
    if (domain_memory == NULL) {
        std::cerr << "Failed to allocate domain memory." << std::endl;
        return -1;
    }

    domain = ecrt_master_create_domain(master, DOMAIN_IDX, domain_memory, domain_size);
    if (domain == NULL) {
        std::cerr << "Failed to create domain." << std::endl;
        return -1;
    }

    /* Get drive configuration */
    drive_config = ecrt_master_slave_config(master, DRIVE_IDX);
    if (drive_config == NULL) {
        std::cerr << "Failed to get drive configuration." << std::endl;
        return -1;
    }

    /* Check vendor ID and product code */
    ecrt_slave_config_get_vendor_id(drive_config, &vendor_id);
    ecrt_slave_config_get_product_code(drive_config, &product_code);
    if (vendor_id != VENDOR_ID || product_code != PRODUCT_CODE) {
        std::cerr << "Invalid vendor ID or product code." << std::endl;
        return -1;
    }

    /* Initialize drive */
    drive.domain_memory = domain_memory;
    drive.domain = domain;
    drive.drive_config = drive_config;
    drive.controlword = CONTROLWORD;
    drive.statusword = STATUSWORD;
    drive.target_pos = TARGET_POS;
    drive.target_vel = TARGET_VEL;
    drive.profile_vel = PROFILE_VEL;
    drive.profile_acc = PROFILE_ACC;
    drive.profile_dec = PROFILE_DEC;
    drive.modes_of_ope = MODES_OF_OPE;
    drive.state = STATE_INIT;

    /* Add drive to list */
    drives.push_back(drive);

    /* Configure EtherCAT network */
    if (ecrt_domain_reg_pdo_entry_list(domain, drive_config->first_out_pdo, NULL) < 0) {
        std::cerr << "Failed to register PDO entry list." << std::endl;
        return -1;
    }

    if (ecrt_domain_reg_pdo_entry_list(domain, drive_config->first_in_pdo, NULL) < 0) {
        std::cerr << "Failed to register PDO entry list." << std::endl;
        return -1;
    }

    if (ecrt_domain_activate(domain) != 0) {
        std::cerr << "Failed to activate domain." << std::endl;
        return -1;
    }

    /* Configure drive */
    if (ecrt_master_sdo_download(drive.drive_config, 0x6060, 0x00, &drive.modes_of_ope, sizeof(drive.modes_of_ope)) != sizeof(drive.modes_of_ope)) {
        std::cerr << "Failed to set modes of operation." << std::endl;
        return -1;
    }

    if (ecrt_master_sdo_download(drive.drive_config, 0x6081, 0x00, &drive.profile_vel, sizeof(drive.profile_vel)) != sizeof(drive.profile_vel)) {
        std::cerr << "Failed to set profile velocity." << std::endl;
        return -1;
    }

    if (ecrt_master_sdo_download(drive.drive_config, 0x6083, 0x00, &drive.profile_acc, sizeof(drive.profile_acc)) != sizeof(drive.profile_acc)) {
        std::cerr << "Failed to set profile acceleration." << std::endl;
        return -1;
    }

    if (ecrt_master_sdo_download(drive.drive_config, 0x6084, 0x00, &drive.profile_dec, sizeof(drive.profile_dec)) != sizeof(drive.profile_dec)) {
        std::cerr << "Failed to set profile deceleration." << std::endl;
        return -1;
    }

    /* Set drive to pre-operational state */
    if (ecrt_slave_config_pdos(drive.drive_config, 1, NULL, NULL) < 0) {
        std::cerr << "Failed to configure PDOs." << std::endl;
        return -1;
    }

    if (ecrt_master_sdo_download(drive.drive_config, 0x6040, 0x00, &(drive.controlword), sizeof(drive.controlword)) != sizeof(drive.controlword)) {
        std::cerr << "Failed to set control word." << std::endl;
        return -1;
    }

    /* Start process data exchange */
    if (ecrt_domain_queue(domain) < 0) {
        std::cerr << "Failed to queue domain." << std::endl;
        return -1;
    }

    if (ecrt_master_activate(master) != 0) {
        std::cerr << "Failed to activate EtherCAT master." << std::endl;
        return -1;
    }

    /* Wait for drive to enter pre-operational state */
    while (drive.state == STATE_INIT) {
        check_domain_state();
    }

    /* Set drive to operational state */
    if (ecrt_master_sdo_download(drive.drive_config, 0x6040, 0x00, &(drive.controlword), sizeof(drive.controlword)) != sizeof(drive.controlword)) {
        std::cerr << "Failed to set control word." << std::endl;
        return -1;
    }

    /* Wait for drive to enter operational state */
    while (drive.state != STATE_OP) {
        check_domain_state();
    }

    /* Set target position and velocity */
    drive.target_pos = 10000;
    drive.target_vel = 1000;

    if (ecrt_master_sdo_download(drive.drive_config, drive.target_pos, 0x00, &(drive.target_pos), sizeof(drive.target_pos)) != sizeof(drive.target_pos)) {
        std::cerr << "Failed to set target position." << std::endl;
        return -1;
    }

    if (ecrt_master_sdo_download(drive.drive_config, drive.target_vel, 0x00, &(drive.target_vel), sizeof(drive.target_vel)) != sizeof(drive.target_vel)) {
        std::cerr << "Failed to set target velocity." << std::endl;
        return -1;
    }

    /* Enable drive */
    drive.controlword |= (1 << 0);

    if (ecrt_master_sdo_download(drive.drive_config, 0x6040, 0x00, &(drive.controlword), sizeof(drive.controlword)) != sizeof(drive.controlword)) {
        std::cerr << "Failed to set control word." << std::endl;
        return -1;
    }

    /* Wait for drive to reach target position */
    while ((drive.state == STATE_OP) && ((drive.statusword & (1 << 10)) == 0)) {
        check_domain_state();
    }

    /* Disable drive */
    drive.controlword &= ~(1 << 0);

    if (ecrt_master_sdo_download(drive.drive_config, 0x6040, 0x00, &(drive.controlword), sizeof(drive.controlword)) != sizeof(drive.controlword)) {
        std::cerr << "Failed to set control word." << std::endl;
        return -1;
    }

    /* Set drive to pre-operational state */
    if (ecrt_master_sdo_download(drive.drive_config, 0x6040, 0x00, &(drive.controlword), sizeof(drive.controlword)) != sizeof(drive.controlword)) {
        std::cerr << "Failed to set control word." << std::endl;
        return -1;
    }

    /* Stop process data exchange */
    if (ecrt_master_deactivate(master) != 0) {
        std::cerr << "Failed to deactivate EtherCAT master." << std::endl;
        return -1;
    }

    if (ecrt_master_close(master) != 0) {
        std::cerr << "Failed to close EtherCAT master." << std::endl;
        return -1;
    }

    free(domain_memory);
    return 0;
}

static void check_domain_state(void) {
    ec_domain_state_t domain_state;

    ecrt_master_receive(master);
    ecrt_domain_process(domain);

    for (size_t i = 0; i < drives.size(); i++) {
        domain_state = ecrt_domain_state(drives[i].domain);

        if (domain_state.working_counter != drives[i].state) {
            drives[i].state = domain_state.working_counter;
        }
    }
}

这个示例程序演示了如何使用EtherCAT总线控制雷赛DM3C。程序通过EtherCAT主机与从设备通信，使用SDO（服务数据对象）和PDO（过程数据对象）来读取和写入驱动器状态和参数。程序调用了ecrt库的函数来进行EtherCAT总线的操作。首先，程序初始化EtherCAT主机，创建一个EtherCAT域，并添加一个驱动器。然后，程序定义了一系列驱动器的控制面（例如控制字、状态字、目标位置、目标速度等），并将驱动器配置为运行前的状态。程序启动数据交换过程，等待驱动器进入操作状态。最后，程序通过SDO设置了驱动器的目标位置和目标速度，并将驱动器启用。程序等待驱动器到达目标位置，然后禁用驱动器。