完整代码
#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 0x0000066f, 0x60380006
// 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.operational)
printf(“sc_estun_state: %soperational.n”,s.operational ? “” : “Not ”);
sc_estun_state = s;
}
/****************************************************************************/
void ReleaseMaster()
{
if(master)
{
printf(“xenomai End of Program, release mastern”);
ecrt_release_master(master);
master = NULL;
}
}
/****************************************************************************/
int ActivateMaster()
{
int ret;
printf(“xenomai Requesting master.。.n”);
if(master)
return 0;
master = ecrt_request_master(0);
if (!master) {
return -1;
}
ConfigPDO();
// configure SYNC signals for this slave
ecrt_slave_config_dc(sc_estun, 0x0300, 1000000, 0, 0, 0);
// ecrt_slave_config_dc(sc_estun, 0x0300, 4000000, 4000000/2, 0, 0);
ecrt_master_application_time(master, system_time_ns());
ret = ecrt_master_select_reference_clock(master, NULL);
if (ret 《 0) {
fprintf(stderr, “xenomai Failed to select reference clock: %sn”,
strerror(-ret));
return ret;
}
/********************/
printf(“xenomai Activating master.。.n”);
if (ecrt_master_activate(master)) {
printf(“xenomai Activating master.。.failedn”);
return -1;
}
/********************/
if (!(domainInput_pd = ecrt_domain_data(domainServoInput))) {
fprintf(stderr, “xenomai Failed to get domain data pointer.n”);
return -1;
}
if (!(domainOutput_pd = ecrt_domain_data(domainServoOutput))) {
fprintf(stderr, “xenomai Failed to get domain data pointer.n”);
return -1;
}
printf(“xenomai Activating master.。.successn”);
return 0;
}
/****************************************************************************/
void DriverEtherCAT()
{
static int curpos = 0;
static int curpos_offset = 0;
static int i = 0;
//处于刚开机(需要等待其他操作完成),返回等待下次周期
if(gSysRunning.m_gWorkStatus == SYS_WORKING_POWER_ON)
return ;
static int cycle_counter = 0;
cycle_counter++;
if(cycle_counter 》= 600*1000){
cycle_counter = 0;
run = 0;
}
// receive EtherCAT frames
ecrt_master_receive(master);
ecrt_domain_process(domainServoOutput);
ecrt_domain_process(domainServoInput);
rt_check_domain_state();
if (!(cycle_counter % 500)) {
rt_check_master_state();
check_slave_config_states();
}
//状态机操作
switch (gSysRunning.m_gWorkStatus)
{
case SYS_WORKING_SAFE_MODE:{
//检查主站是否处于 OP 模式, 若不是,则调整为 OP 模式
rt_check_master_state();
check_slave_config_states();
if((master_state.al_states & ETHERCAT_STATUS_OP))
{
int tmp = true;
if(sc_estun_state.al_state != ETHERCAT_STATUS_OP)
{
tmp = false;
break ;
}
if(tmp)
{
ecstate = 0;
gSysRunning.m_gWorkStatus = SYS_WORKING_OP_MODE;
printf(“xenomai SYS_WORKING_OP_MODEn”);
EC_WRITE_U16(domainOutput_pd + cntlwd, 0x80); //错误复位
}
}
}break;
case SYS_WORKING_OP_MODE:
{
EC_WRITE_U8(domainOutput_pd + modes_of_operation, 8);
cur_mode= EC_READ_U8(domainInput_pd + modes_of_operation_display);
printf(“curMode: %dt”, cur_mode); //当前操作模式
cur_status = EC_READ_U16(domainInput_pd + status);
printf(“curStatus: %dn”, cur_status);
if((cur_status & 0x004f) == 0x0040)
{
EC_WRITE_U16(domainOutput_pd + cntlwd, 0x06);
// cur_mode= EC_READ_U8(domainInput_pd + modes_of_operation_display);
// printf(“curMode: %dn”, cur_mode); //当前操作模式
printf(“0x06n”);
}
else if((cur_status & 0x006f) == 0x0021)
{
EC_WRITE_U16(domainOutput_pd + cntlwd, 0x07);
printf(“0x07n”);
}
else if((cur_status & 0x006f) == 0x023)
{
EC_WRITE_U16(domainOutput_pd + cntlwd, 0x0F);
printf(“0x0Fn”);
}
else if((cur_status & 0x006f) == 0x0027)
{
EC_WRITE_U16(domainOutput_pd + cntlwd, 0x001f);
printf(“0x1fn”);
curpos = EC_READ_S32(domainInput_pd + actpos);
// EC_WRITE_S32(domainOutput_pd + ipData, EC_READ_S32(domainInput_pd + actpos));
printf(“x@rtITP 》》》 Axis current position = %dn”, curpos);
int tmp = true;
if((EC_READ_U16(domainInput_pd + status) & (STATUS_SERVO_ENABLE_BIT)) == 0)
{
tmp = false;
break ;
}
if(tmp)
{
ecstate = 0;
gSysRunning.m_gWorkStatus = SYS_WORKING_IDLE_STATUS;
printf(“xenomai SYS_WORKING_IDLE_STATUSn”);
}
}
}break;
default:
{
cur_status = EC_READ_U16(domainInput_pd + status);
if (!(cycle_counter % 1000)) {
printf(“curpos = %dt”,curpos);
printf(“actpos.。. %dn”,EC_READ_S32(domainInput_pd + actpos));
}
curpos += 10000;
EC_WRITE_S32(domainOutput_pd + ipData, curpos);
}break;
}
// write application time to master
ecrt_master_application_time(master, system_time_ns());
ecrt_master_sync_reference_clock(master);
ecrt_master_sync_slave_clocks(master);
// send process data
ecrt_domain_queue(domainServoOutput);
ecrt_domain_queue(domainServoInput);
ecrt_master_send(master);
}
/****************************************************************************/
void InterpolationThread(void *arg)
{
RTIME wait, previous;
previous = rt_timer_read();
wait = previous;
while (run) {
wait += 1000000; //1ms
//Delay the calling task (absolute).Delay the execution of the calling task until a given date is reached.
rt_task_sleep_until(wait);
DriverEtherCAT();
}
}
/****************************************************************************
* Signal handler
***************************************************************************/
void signal_handler(int sig)
{
run = 0;
}
/****************************************************************************
* Main function
***************************************************************************/
int main(int argc, char *argv[])
{
int ret;
/* Perform auto-init of rt_print buffers if the task doesn‘t do so */
rt_print_auto_init(1);
signal(SIGTERM, signal_handler);
signal(SIGINT, signal_handler);
mlockall(MCL_CURRENT | MCL_FUTURE);
gSysRunning.m_gWorkStatus = SYS_WORKING_POWER_ON;
if(gSysRunning.m_gWorkStatus == SYS_WORKING_POWER_ON)
{
ActivateMaster();
ecstate = 0;
gSysRunning.m_gWorkStatus = SYS_WORKING_SAFE_MODE;
printf(“xenomai SYS_WORKING_SAFE_MODEn”);
}
ret = rt_task_create(&InterpolationTask, “InterpolationTask”, 0, 99, T_FPU);
if (ret 《 0) {
fprintf(stderr, “xenomai Failed to create task: %sn”, strerror(-ret));
return -1;
}
printf(“Starting InterpolationTask.。.n”);
ret = rt_task_start(&InterpolationTask, &InterpolationThread, NULL);
if (ret 《 0) {
fprintf(stderr, “xenomai Failed to start task: %sn”, strerror(-ret));
return -1;
}
while (run) {
rt_task_sleep(50000000);
}
printf(“xenomai Deleting realtime InterpolationTask task.。.n”);
rt_task_delete(&InterpolationTask);
ReleaseMaster();
return 0;
}
完整代码
#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 0x0000066f, 0x60380006
// 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.operational)
printf(“sc_estun_state: %soperational.n”,s.operational ? “” : “Not ”);
sc_estun_state = s;
}
/****************************************************************************/
void ReleaseMaster()
{
if(master)
{
printf(“xenomai End of Program, release mastern”);
ecrt_release_master(master);
master = NULL;
}
}
/****************************************************************************/
int ActivateMaster()
{
int ret;
printf(“xenomai Requesting master.。.n”);
if(master)
return 0;
master = ecrt_request_master(0);
if (!master) {
return -1;
}
ConfigPDO();
// configure SYNC signals for this slave
ecrt_slave_config_dc(sc_estun, 0x0300, 1000000, 0, 0, 0);
// ecrt_slave_config_dc(sc_estun, 0x0300, 4000000, 4000000/2, 0, 0);
ecrt_master_application_time(master, system_time_ns());
ret = ecrt_master_select_reference_clock(master, NULL);
if (ret 《 0) {
fprintf(stderr, “xenomai Failed to select reference clock: %sn”,
strerror(-ret));
return ret;
}
/********************/
printf(“xenomai Activating master.。.n”);
if (ecrt_master_activate(master)) {
printf(“xenomai Activating master.。.failedn”);
return -1;
}
/********************/
if (!(domainInput_pd = ecrt_domain_data(domainServoInput))) {
fprintf(stderr, “xenomai Failed to get domain data pointer.n”);
return -1;
}
if (!(domainOutput_pd = ecrt_domain_data(domainServoOutput))) {
fprintf(stderr, “xenomai Failed to get domain data pointer.n”);
return -1;
}
printf(“xenomai Activating master.。.successn”);
return 0;
}
/****************************************************************************/
void DriverEtherCAT()
{
static int curpos = 0;
static int curpos_offset = 0;
static int i = 0;
//处于刚开机(需要等待其他操作完成),返回等待下次周期
if(gSysRunning.m_gWorkStatus == SYS_WORKING_POWER_ON)
return ;
static int cycle_counter = 0;
cycle_counter++;
if(cycle_counter 》= 600*1000){
cycle_counter = 0;
run = 0;
}
// receive EtherCAT frames
ecrt_master_receive(master);
ecrt_domain_process(domainServoOutput);
ecrt_domain_process(domainServoInput);
rt_check_domain_state();
if (!(cycle_counter % 500)) {
rt_check_master_state();
check_slave_config_states();
}
//状态机操作
switch (gSysRunning.m_gWorkStatus)
{
case SYS_WORKING_SAFE_MODE:{
//检查主站是否处于 OP 模式, 若不是,则调整为 OP 模式
rt_check_master_state();
check_slave_config_states();
if((master_state.al_states & ETHERCAT_STATUS_OP))
{
int tmp = true;
if(sc_estun_state.al_state != ETHERCAT_STATUS_OP)
{
tmp = false;
break ;
}
if(tmp)
{
ecstate = 0;
gSysRunning.m_gWorkStatus = SYS_WORKING_OP_MODE;
printf(“xenomai SYS_WORKING_OP_MODEn”);
EC_WRITE_U16(domainOutput_pd + cntlwd, 0x80); //错误复位
}
}
}break;
case SYS_WORKING_OP_MODE:
{
EC_WRITE_U8(domainOutput_pd + modes_of_operation, 8);
cur_mode= EC_READ_U8(domainInput_pd + modes_of_operation_display);
printf(“curMode: %dt”, cur_mode); //当前操作模式
cur_status = EC_READ_U16(domainInput_pd + status);
printf(“curStatus: %dn”, cur_status);
if((cur_status & 0x004f) == 0x0040)
{
EC_WRITE_U16(domainOutput_pd + cntlwd, 0x06);
// cur_mode= EC_READ_U8(domainInput_pd + modes_of_operation_display);
// printf(“curMode: %dn”, cur_mode); //当前操作模式
printf(“0x06n”);
}
else if((cur_status & 0x006f) == 0x0021)
{
EC_WRITE_U16(domainOutput_pd + cntlwd, 0x07);
printf(“0x07n”);
}
else if((cur_status & 0x006f) == 0x023)
{
EC_WRITE_U16(domainOutput_pd + cntlwd, 0x0F);
printf(“0x0Fn”);
}
else if((cur_status & 0x006f) == 0x0027)
{
EC_WRITE_U16(domainOutput_pd + cntlwd, 0x001f);
printf(“0x1fn”);
curpos = EC_READ_S32(domainInput_pd + actpos);
// EC_WRITE_S32(domainOutput_pd + ipData, EC_READ_S32(domainInput_pd + actpos));
printf(“x@rtITP 》》》 Axis current position = %dn”, curpos);
int tmp = true;
if((EC_READ_U16(domainInput_pd + status) & (STATUS_SERVO_ENABLE_BIT)) == 0)
{
tmp = false;
break ;
}
if(tmp)
{
ecstate = 0;
gSysRunning.m_gWorkStatus = SYS_WORKING_IDLE_STATUS;
printf(“xenomai SYS_WORKING_IDLE_STATUSn”);
}
}
}break;
default:
{
cur_status = EC_READ_U16(domainInput_pd + status);
if (!(cycle_counter % 1000)) {
printf(“curpos = %dt”,curpos);
printf(“actpos.。. %dn”,EC_READ_S32(domainInput_pd + actpos));
}
curpos += 10000;
EC_WRITE_S32(domainOutput_pd + ipData, curpos);
}break;
}
// write application time to master
ecrt_master_application_time(master, system_time_ns());
ecrt_master_sync_reference_clock(master);
ecrt_master_sync_slave_clocks(master);
// send process data
ecrt_domain_queue(domainServoOutput);
ecrt_domain_queue(domainServoInput);
ecrt_master_send(master);
}
/****************************************************************************/
void InterpolationThread(void *arg)
{
RTIME wait, previous;
previous = rt_timer_read();
wait = previous;
while (run) {
wait += 1000000; //1ms
//Delay the calling task (absolute).Delay the execution of the calling task until a given date is reached.
rt_task_sleep_until(wait);
DriverEtherCAT();
}
}
/****************************************************************************
* Signal handler
***************************************************************************/
void signal_handler(int sig)
{
run = 0;
}
/****************************************************************************
* Main function
***************************************************************************/
int main(int argc, char *argv[])
{
int ret;
/* Perform auto-init of rt_print buffers if the task doesn‘t do so */
rt_print_auto_init(1);
signal(SIGTERM, signal_handler);
signal(SIGINT, signal_handler);
mlockall(MCL_CURRENT | MCL_FUTURE);
gSysRunning.m_gWorkStatus = SYS_WORKING_POWER_ON;
if(gSysRunning.m_gWorkStatus == SYS_WORKING_POWER_ON)
{
ActivateMaster();
ecstate = 0;
gSysRunning.m_gWorkStatus = SYS_WORKING_SAFE_MODE;
printf(“xenomai SYS_WORKING_SAFE_MODEn”);
}
ret = rt_task_create(&InterpolationTask, “InterpolationTask”, 0, 99, T_FPU);
if (ret 《 0) {
fprintf(stderr, “xenomai Failed to create task: %sn”, strerror(-ret));
return -1;
}
printf(“Starting InterpolationTask.。.n”);
ret = rt_task_start(&InterpolationTask, &InterpolationThread, NULL);
if (ret 《 0) {
fprintf(stderr, “xenomai Failed to start task: %sn”, strerror(-ret));
return -1;
}
while (run) {
rt_task_sleep(50000000);
}
printf(“xenomai Deleting realtime InterpolationTask task.。.n”);
rt_task_delete(&InterpolationTask);
ReleaseMaster();
return 0;
}
举报