完整代码

#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 = {};//domainServoInputecrt_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;//domainServoOutputecrt_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 master\n");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 slaveecrt_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: %s\n",strerror(-ret));return ret;}/********************/printf("xenomai Activating master...\n");if (ecrt_master_activate(master)) {printf("xenomai Activating master...failed\n");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...success\n");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 framesecrt_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_MODE\n");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: %d\t", cur_mode); //当前操作模式cur_status = EC_READ_U16(domainInput_pd + status);printf("curStatus: %d\n", 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: %d\n", cur_mode); //当前操作模式printf("0x06\n");}else if((cur_status & 0x006f) == 0x0021){EC_WRITE_U16(domainOutput_pd + cntlwd, 0x07);printf("0x07\n");}else if((cur_status & 0x006f) == 0x023){EC_WRITE_U16(domainOutput_pd + cntlwd, 0x0F);printf("0x0F\n");}else if((cur_status & 0x006f) == 0x0027){EC_WRITE_U16(domainOutput_pd + cntlwd, 0x001f);printf("0x1f\n");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 = %d\n", 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_STATUS\n");}}}break;default:{cur_status = EC_READ_U16(domainInput_pd + status);if (!(cycle_counter % 1000)) {printf("curpos = %d\t",curpos);printf("actpos... %d\n",EC_READ_S32(domainInput_pd + actpos));}curpos += 10000;EC_WRITE_S32(domainOutput_pd + ipData, curpos);}break;}// write application time to masterecrt_master_application_time(master, system_time_ns());ecrt_master_sync_reference_clock(master);   ecrt_master_sync_slave_clocks(master);      // send process dataecrt_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_MODE\n"); }ret = rt_task_create(&InterpolationTask, "InterpolationTask", 0, 99, T_FPU);if (ret < 0) {fprintf(stderr, "xenomai Failed to create task: %s\n", 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: %s\n", strerror(-ret));return -1;}while (run) {rt_task_sleep(50000000);}printf("xenomai Deleting realtime InterpolationTask task...\n");rt_task_delete(&InterpolationTask);ReleaseMaster();return 0;
}

测试

EtherCAT igh主站控制松下伺服（csp模式）相关推荐

EtherCAT igh主站控制埃斯顿伺服（csp模式）
完整代码算了.干脆直接贴代码了.最近都在搞EtherCAT主站.从站的应用,过段日子再分享一些项目中基础的东西.驱动伺服主要还是参考厂商提供的EtherCAT伺服参考手册,基本上都采用CIA402协 ...
EtherCAT igh主站控制埃斯顿伺服回零
完整代码 #include <errno.h> #include <signal.h> #include <stdio.h> #include <string ...
FX3U控制松下服务器位置不准,三菱FX3UPLC如何控制松下伺服_.docx
PAGE PAGE # 三菱 FX3U PLC如何控制松下伺服目录 TOC \o "1-5" \h \z \o "Current Document" 实现的功 ...
三菱 FX5U PLC 4轴程序。控制松下伺服3个，步进电机一个
三菱 FX5U PLC 4轴程序. 控制松下伺服3个, 步进电机一个, 四轴自动堆垛码垛设备程序, 回原点动作用专用的原点回归指令写的, 手动运行用三菱相对定位指令写的 , 自动运行用绝对定位指令写的 ...
三菱 FX5U PLC 4轴程序控制松下伺服3个，步进电机一个
三菱 FX5U PLC 4轴程序. 控制松下伺服3个, 步进电机一个, 四轴自动堆垛码垛设备程序, 回原点动作用专用的原点回归指令写的, 手动运行用三菱相对定位指令写的 , 自动运行用绝对定位指令写的 ...
倍福TwinCAT(贝福Beckhoff)应用教程12.1 TwinCAT控制松下伺服连接和试运行
首先是用松下伺服自带的软件可以测试运行(驱动器,电机都连接好,然后用USB线连接到松下伺服驱动器的X1口),打开调试软件会自动提示连接到伺服一般需要对驱动器清除绝对值编码器数据(驱动器可能报错40错 ...
三菱5uplc伺服电机指令_三菱FX3U PLC如何控制松下伺服
一.实现的功能及应用的场合通过PLC的不同指令,发送两轴伺服电机所需要的速度与位置的频率和数量来实现电机的定位运行.JOG运行,适用于数控机床.印刷设备.包装设备.纺织设备.激光加工设备.机器人.自 ...
倍福--控制松下伺服报错x4263
用CX2020控制松下Ethercat伺服,松下伺服使能之后NC在界面无法点动报错0x4263.本文根据现场解决方案进行总结. 操作流程 1.1. 问题描述在客户现场,用CX2020控制松下Ethe ...
倍福TwinCAT(贝福Beckhoff)应用教程13.3 TwinCAT控制松下伺服 NC配合完整上位
这是TwinCAT教程的最后一节,简单讲述了以C#为上位,通过ADS控制TwinCAT下位,实现完整控制两轴模组的功能.可以发现,在上位层已经没有了运动控制的代码,不管是要执行哪种运动,无非是把目标参 ...

EtherCAT igh主站控制松下伺服（csp模式）

完整代码

测试

EtherCAT igh主站控制松下伺服（csp模式）相关推荐

最新文章

热门文章