igh实时控制电机动起来的一个小项目，包括编译环境，及代码

#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 0x00681168,0x00464445/*Vendor ID, product code*/ // 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.operationa