IGH EtherCAT应用层控制电机代码

在主站配置好之后，连接从站，我用的是雷赛的伺服，对于大部分来说改个pid，vid，应该就可以用，这个是用的pv模式
应该还是容易懂的，我把一些用不到的代码给阉割了

/**
  * compile : gcc test.c -o test -I/opt/etherlab/include -L/opt/etherlab/lib -lethercat
  */
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

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

#include "ecrt.h"

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

/*Application Parameters*/
#define TASK_FREQUENCY          10 /*Hz*/
#define TARGET_VELOCITY         110000 /*target velocity*/
#define PROFILE_VELOCITY            3   /*Operation mode for 0x6060:0*/

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

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

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

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

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

/*Process Data*/
static uint8_t *domain1_pd = NULL;

#define DM3E         0,0                        /*EtherCAT address on the bus*/

#define VID_PID           0x00004321,0x00008100   /*Vendor ID, product code*/

/*Offsets for PDO entries*/
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[] = {
    {DM3E, VID_PID, 0x6040, 0, &offset.ctrl_word},
    {DM3E, VID_PID, 0x6060, 0, &offset.operation_mode },
    {DM3E, VID_PID, 0x60FF, 0, &offset.target_velocity},
    {DM3E, VID_PID, 0x6041, 0, &offset.status_word},
    {DM3E, VID_PID, 0x606C, 0, &offset.current_velocity},
    {DM3E, VID_PID, 0x6064, 0, &offset.current_position},
    {}
};

/***************************************************************************/
/*Config PDOs*/
static ec_pdo_entry_info_t device_pdo_entries[] = {
    /*RxPdo 0x1600*/
    {0x6040, 0x00, 16},
    {0x6060, 0x00, 8 }, 
    {0x60FF, 0x00, 32},
    /*TxPdo 0x1A00*/
    {0x6041, 0x00, 16},
    {0x606C, 0x00, 32},
    {0x6064, 0x00, 32}
};

static ec_pdo_info_t device_pdos[] = {
    //RxPdo
    {0x1600, 3, device_pdo_entries + 0 },
    //TxPdo
    {0x1A00, 3, device_pdo_entries + 3 }
};

static ec_sync_info_t device_syncs[] = {
    { 0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE },
    { 1, EC_DIR_INPUT, 0, NULL, 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(void)
{
    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(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, &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;
}

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

void cyclic_task()
{
    static uint16_t command=0x004F;//用来帮助判断状态字的值
    uint16_t    status;
    /*Receive process data*/
    ecrt_master_receive(master);
    ecrt_domain_process(domain1);
    /*Check process data state(optional)*/
    check_domain1_state();

    //Check for master state
    check_master_state();
    //Check for slave configuration state(s)
    check_slave_config_states();
    /*Read state*/
    status = EC_READ_U16(domain1_pd + offset.status_word);//读取状态字

    //DS402 CANOpen over EtherCAT status machine
    if( (status & command) == 0x0040 )
    {
        EC_WRITE_U16(domain1_pd + offset.ctrl_word, 0x0006 );
        EC_WRITE_S8(domain1_pd + offset.operation_mode, PROFILE_VELOCITY);
//设置控制模式
        command = 0x006F;
    }

    else if( (status & command) == 0x0021)
    {
        EC_WRITE_U16(domain1_pd + offset.ctrl_word, 0x0007 );
        command = 0x006F;
    }

    else if( (status & command) == 0x0023)
    {
        EC_WRITE_U16(domain1_pd + offset.ctrl_word, 0x000f );
        command = 0x006F;
    }
    //operation enabled

    else if( (status & command) == 0x0027)
    {
        EC_WRITE_S32(domain1_pd + offset.target_velocity, TARGET_VELOCITY);
        EC_WRITE_U16(domain1_pd + offset.ctrl_word, 0x001f );
    }

    /*Send process data*/
    ecrt_domain_queue(domain1);
    ecrt_master_send(master);
}

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

int main(int argc, char **argv)
{
	 printf("Requesting master...\n");
    master = ecrt_request_master(0);
    if (!master)
    {
        exit(EXIT_FAILURE);
    }
    
    domain1 = ecrt_master_create_domain(master);
    if (!domain1)
    {
        exit(EXIT_FAILURE);
    }
    if (!(sc = ecrt_master_slave_config(master, DM3E, VID_PID)))
    {
        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);
    }
   
    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");

    while (1) 
    {
        usleep(100000/TASK_FREQUENCY);
        cyclic_task();
    }
    return EXIT_SUCCESS;
}