igh ethercat程序雷赛伺服示例

以下是一个使用IgH EtherCAT驱动器控制雷赛伺服的示例程序：

#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <pthread.h>
#include <sched.h>
#include <sys/mman.h>
#include <time.h>

#include "ecrt.h"

#define FREQUENCY 1000
#define PERIOD_NS (1000000000 / FREQUENCY)

// EtherCAT Master
static ec_master_t *master = NULL;
// EtherCAT Slave
static ec_slave_config_t *slave = NULL;
// EtherCAT Domain
static ec_domain_t *domain = NULL;
// EtherCAT Master Clock
static ec_master_clock_t *clock = NULL;

// Sync Manager
static ec_sync_info_t sync_info = {
    .dir = EC_DIR_OUTPUT,
    .n = 0,
    .type = EC_SYNC_CYCLOP,
    .size = 0,
    .offset = 0,
    .config_index = 0
};
static ec_sync_info_t *syncs[] = { &sync_info, NULL };

// Process Data
struct {
    uint16_t status_word;
    int32_t target_position;
    int32_t target_velocity;
    int16_t target_torque;
} __attribute__((packed)) pd;

// Process Data Mapping
static ec_pdo_entry_info_t pdo_entries[] = {
    { 0x6041, 0x00, 16 }, // Control Word
    { 0x6040, 0x00, 16 }, // Status Word
    { 0x607A, 0x00, 32 }, // Target Position
    { 0x60FF, 0x00, 32 }, // Target Velocity
    { 0x6071, 0x00, 16 }, // Target Torque
};
static ec_pdo_info_t pdo_info = {
    .n_entries = 5,
    .entry = pdo_entries
};
static ec_sync_pdo_entry_t sync_pdos[] = {
    { 0x03A0, 0x01,  &pdo_info, 0 },
    { 0x03A0, 0x02,  &pdo_info, 0 },
    { 0x03A0, 0x03,  &pdo_info, 0 },
    { 0x03A0, 0x04,  &pdo_info, 0 },
    { 0x03A0, 0x05,  &pdo_info, 0 },
    { 0x03A0, 0x06,  &pdo_info, 0 },
    { 0x03A0, 0x07,  &pdo_info, 0 },
    { 0x03A0, 0x08,  &pdo_info, 0 },
};
static ec_sync_info_t sync_pdos_info[] = {
    { .n = 0, .type = EC_SYNC_PDO, .size = 0, .offset = 0, .config_index = 0xff },
    { .n = 8, .type = EC_SYNC_PDO, .size = sizeof(pd), .offset = 0, .config_index = 0 },
    { .n = 0, .type = EC_SYNC_END, .size = 0, .offset = 0, .config_index = 0 }
};

// Thread
static pthread_t thread;
static int thread_run = 1;

// Application
void *application(void *arg) {
    struct timespec ts;
    uint32_t cycle_ns = PERIOD_NS;
    int32_t position;
    uint16_t status;
    int wkc;
    int error;
    int32_t pos;
    int32_t vel;
    int16_t tor;

    // Set thread priority
    struct sched_param param = { .sched_priority = 99 };
    pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);

    // Run EtherCAT Master
    ecrt_master_activate(master);
    ecrt_master_application_time(master, cycle_ns, 0);
    ecrt_master_sync_reference_clock(master);
    ecrt_master_sync_slave_clocks(master);

    while (thread_run) {
        // Wait for next cycle
        clock_gettime(CLOCK_MONOTONIC, &ts);
        ts.tv_nsec += cycle_ns;
        if (ts.tv_nsec >= 1000000000) {
            ts.tv_sec++;
            ts.tv_nsec -= 1000000000;
        }
        pthread_mutex_lock(&ecrt_master_mutex);
        ecrt_master_application_time(master, cycle_ns, 0);
        ecrt_master_sync_reference_clock(master);
        wkc = ecrt_master_sync_slave_clocks(master);
        pthread_mutex_unlock(&ecrt_master_mutex);

        // Read current position
        pthread_mutex_lock(&ecrt_domain_mutex);
        position = EC_READ_S32(domain->io_map + 0);
        pthread_mutex_unlock(&ecrt_domain_mutex);

        // Write target position, velocity and torque
        pd.target_position = position;
        pd.target_velocity = 1000;
        pd.target_torque = 0;

        // Send process data
        pthread_mutex_lock(&ecrt_domain_mutex);
        ecrt_domain_queue(domain);
        pthread_mutex_unlock(&ecrt_domain_mutex);

        // Wait for next cycle
        clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, NULL);

        // Receive process data
        pthread_mutex_lock(&ecrt_domain_mutex);
        ecrt_domain_process(domain);
        status = EC_READ_U16(domain->io_map + 2);
        pos = EC_READ_S32(domain->io_map + 4);
        vel = EC_READ_S32(domain->io_map + 8);
        tor = EC_READ_S16(domain->io_map + 12);
        pthread_mutex_unlock(&ecrt_domain_mutex);

        printf("Status: 0x%04X, Position: %d, Velocity: %d, Torque: %d\n", status, pos, vel, tor);

        // Check for errors
        error = ecrt_master_error(master);
        if (error) {
            printf("Error: %s (%d)\n", ecrt_master_strerror(error), error);
            break;
        }

        // Check for watchdog
        if (wkc < sync_pdos_info[0].n * ec_slave_config_reg_pdo_entry_count(slave, 0)) {
            printf("Watchdog!\n");
            thread_run = 0;
        }
    }

    // Stop EtherCAT Master
    pthread_mutex_lock(&ecrt_master_mutex);
    ecrt_master_deactivate(master);
    pthread_mutex_unlock(&ecrt_master_mutex);

    return NULL;
}

// Main
int main(int argc, char **argv) {
    int result;

    // Initialize EtherCAT Master
    if (ecrt_master_open(&master)) {
        printf("Failed to open EtherCAT Master!\n");
        return -1;
    }
    if (ecrt_master_open_config(master, NULL)) {
        printf("Failed to open EtherCAT Master configuration!\n");
        ecrt_master_close(master);
        return -1;
    }
    if (ecrt_master_application_ready(master)) {
        printf("Failed to make EtherCAT Master application ready!\n");
        ecrt_master_close(master);
        return -1;
    }

    // Get EtherCAT Slave configuration
    slave = ecrt_master_slave_config(master, 0x03A0, 0x0000);
    if (!slave) {
        printf("Failed to get EtherCAT Slave configuration!\n");
        ecrt_master_close(master);
        return -1;
    }

    // Initialize EtherCAT Domain
    domain = ecrt_master_create_domain(master, &sync_info, syncs);
    if (!domain) {
        printf("Failed to create EtherCAT Domain!\n");
        ecrt_master_close(master);
        return -1;
    }

    // Initialize EtherCAT Master Clock
    clock = ecrt_master_clock(master);
    if (!clock) {
        printf("Failed to initialize EtherCAT Master Clock!\n");
        ecrt_master_close(master);
        return -1;
    }

    // Register process data mapping
    if (ecrt_slave_config_pdos(slave, EC_END, sync_pdos_info)) {
        printf("Failed to register process data mapping!\n");
        ecrt_master_close(master);
        return -1;
    }

    // Configure EtherCAT Slave
    if (ecrt_slave_config_dc(slave, PERIOD_NS, 0, PERIOD_NS / 2, 0, 0)) {
        printf("Failed to configure EtherCAT Slave!\n");
        ecrt_master_close(master);
        return -1;
    }

    // Activate EtherCAT Slave
    if (ecrt_slave_activate(slave)) {
        printf("Failed to activate EtherCAT Slave!\n");
        ecrt_master_close(master);
        return -1;
    }

    // Initialize process data
    pd.status_word = 0;
    pd.target_position = 0;
    pd.target_velocity = 0;
    pd.target_torque = 0;

    // Start thread
    result = pthread_create(&thread, NULL, &application, NULL);
    if (result) {
        printf("Failed to start thread!\n");
        ecrt_slave_deactivate(slave);
        ecrt_master_close(master);
        return -1;
    }

    // Wait for thread
    pthread_join(thread, NULL);

    // Deactivate EtherCAT Slave
    ecrt_slave_deactivate(slave);

    // Deinitialize EtherCAT Master
    ecrt_master_close(master);

    return 0;
}

在该示例程序中，使用了以下步骤来控制雷赛伺服：

1. 初始化EtherCAT Master。
2. 获取EtherCAT Slave配置。
3. 初始化EtherCAT Domain和Sync Manager。
4. 注册Process Data Mapping。
5. 配置EtherCAT Slave。
6. 激活EtherCAT Slave。
7. 启动线程，该线程周期性地更新Process Data，并将其发送到EtherCAT Slave。
8. 等待线程结束。
9. 停止EtherCAT Slave。
10. 关闭EtherCAT Master。

在线程中，使用了以下步骤来更新Process Data并将其发送到EtherCAT Slave：

1. 读取当前位置。
2. 设置目标位置、目标速度和目标力矩。
3. 将Process Data发送到EtherCAT Slave。
4. 等待下一个周期。
5. 接收Process Data。
6. 检查错误和看门狗。

请注意，该示例程序仅供参考，您需要根据实际情况对其进行修改和调整。