EtherCAT igh主站控制3个台达asdaa2伺服转圈圈

2023-11-27 17:20
文章标签 控制 转圈圈 ethercat 伺服 主站 台达 igh asdaa2

本文主要是介绍EtherCAT igh主站控制3个台达asdaa2伺服转圈圈，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

1.查看ASDA的PDO映射

在这里插入图片描述
打开ASDA的Delta_ASDA2-E_rev4-00_XML_TSE_20160620.xml文件
在这里插入图片描述
修改main.c的pdo部分

2.使能伺服

在这里插入图片描述
设置0x6060和0x60C2

    for(int i=0;i<3;i++){ecrt_slave_config_sdo8(sc_asda[i], 0x6060, 0, 8);//设置为csp模式ecrt_slave_config_sdo8(sc_asda[i], 0x60C2, 1, 1);//设置插补周期为1ms}

设置伺服DC，我写了篇文章有谈到这个函数EtherCAT igh源码的ecrt_slave_config_dc（）函数的理解。
https://blog.csdn.net/cln512/article/details/103365240

    ecrt_slave_config_dc(sc_asda[0], 0x0300, 1000000, 0, 0, 0);ecrt_slave_config_dc(sc_asda[1], 0x0300, 1000000, 0, 0, 0);ecrt_slave_config_dc(sc_asda[2], 0x0300, 1000000, 0, 0, 0);

将【Controlword:6040h】依序设定为 (0x06 > 0x07 > 0x0F)，使驱动器 Servo On 并让电机开始运作。

                switch (ecstate){case 1:EC_WRITE_U16(domainOutput_pd + cntlwd[i], 0x80);       break;case 7:curpos = EC_READ_S32(domainInput_pd + actpos[i]);       EC_WRITE_S32(domainOutput_pd + ipData[i], EC_READ_S32(domainInput_pd + actpos[i])); printf("x@rtITP >>> Axis %d current position = %d\n", i, curpos);break;case 9:EC_WRITE_U16(domainOutput_pd + cntlwd[i], 0x06);break;case 11:EC_WRITE_U16(domainOutput_pd + cntlwd[i], 0x07);break;case 13:EC_WRITE_U16(domainOutput_pd + cntlwd[i], 0xF);break;}

3.完整代码

/********************************************************************************  2019-12-02*  三个台达伺服每秒转1圈,100000 pulse/r*  *  ******************************************************************************/
#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 "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_asda[3];
static ec_slave_config_state_t sc_asda_state[3];
/****************************************************************************/
#define asda_Pos0 0, 0
#define asda_Pos1 0, 1
#define asda_Pos2 0, 2
#define asda 0x000001dd, 0x10305070
// offsets for PDO entries
static unsigned int  cntlwd[3];
static unsigned int  ipData[3];
static unsigned int  status[3];
static unsigned int  actpos[3];
static unsigned int  actvel[3];
// process data
ec_pdo_entry_reg_t domainServoOutput_regs[] = {{asda_Pos0, asda, 0x6040, 0x00, &cntlwd[0], NULL},{asda_Pos0, asda, 0x607a, 0x00, &ipData[0], NULL},{asda_Pos1, asda, 0x6040, 0x00, &cntlwd[1], NULL},{asda_Pos1, asda, 0x607a, 0x00, &ipData[1], NULL},{asda_Pos2, asda, 0x6040, 0x00, &cntlwd[2], NULL},{asda_Pos2, asda, 0x607a, 0x00, &ipData[2], NULL},{}
};
ec_pdo_entry_reg_t domainServoInput_regs[] = {{asda_Pos0, asda, 0x6064, 0x00, &actpos[0], NULL},{asda_Pos0, asda, 0x6041, 0x00, &status[0], NULL},{asda_Pos0, asda, 0x606c, 0x00, &actvel[0], NULL},{asda_Pos1, asda, 0x6064, 0x00, &actpos[1], NULL},{asda_Pos1, asda, 0x6041, 0x00, &status[1], NULL},{asda_Pos1, asda, 0x606c, 0x00, &actvel[1], NULL},{asda_Pos2, asda, 0x6064, 0x00, &actpos[2], NULL},{asda_Pos2, asda, 0x6041, 0x00, &status[2], NULL},{asda_Pos2, asda, 0x606c, 0x00, &actvel[2], NULL},{}
};
/****************************************************************************/
/* Master 0, Slave 0* Vendor ID:       0x000001dd* Product code:    0x10305070* Revision number: 0x02040608*/
//《 Delta_ASDA2-E_rev4-00_XML_TSE_20160620.xml》
static ec_pdo_entry_info_t asda_pdo_entries_output[] = {{ 0x6040, 0x00, 16 }, //control word{ 0x607a, 0x00, 32 }  //TargetPosition};static ec_pdo_entry_info_t asda_pdo_entries_input[] = {{ 0x6064, 0x00, 32 }, //actualPosition{ 0x6041, 0x00, 16 }, //status word{ 0x606c, 0x00, 32 }, //Velocity actual value
};//RxPDO
static ec_pdo_info_t asda_pdo_1600[] = {{ 0x1600, 2, asda_pdo_entries_output },
};
//TxPDO
static ec_pdo_info_t asda_pdo_1a00[] = {{ 0x1A00, 3, asda_pdo_entries_input },
};static ec_sync_info_t asda_syncs[] = {
//    { 0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE },
//    { 1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE },{ 2, EC_DIR_OUTPUT, 1, asda_pdo_1600, EC_WD_DISABLE },{ 3, EC_DIR_INPUT, 1, asda_pdo_1a00, 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_asda[0] =ecrt_master_slave_config(master, asda_Pos0, asda);if (!sc_asda[0]) {fprintf(stderr, "Failed to get slave configuration.\n");return -1;}sc_asda[1] =ecrt_master_slave_config(master, asda_Pos1, asda);if (!sc_asda[1]) {fprintf(stderr, "Failed to get slave configuration.\n");return -1;}sc_asda[2] =ecrt_master_slave_config(master, asda_Pos2, asda);if (!sc_asda[2]) {fprintf(stderr, "Failed to get slave configuration.\n");return -1;}/********************/if (ecrt_slave_config_pdos(sc_asda[0], EC_END, asda_syncs)) {fprintf(stderr, "Failed to configure PDOs.\n");return -1;}if (ecrt_slave_config_pdos(sc_asda[1], EC_END, asda_syncs)) {fprintf(stderr, "Failed to configure PDOs.\n");return -1;}if (ecrt_slave_config_pdos(sc_asda[2], EC_END, asda_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");for(int i=0;i<3;i++){ecrt_slave_config_sdo8(sc_asda[i], 0x6060, 0, 8);ecrt_slave_config_sdo8(sc_asda[i], 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_asda[0],&s);if (s.al_state != sc_asda_state[0].al_state)printf("sc_asda_state[0]: State 0x%02X.\n", s.al_state);if (s.online != sc_asda_state[0].online)printf("sc_asda_state[0]: %s.\n", s.online ? "online" : "offline");if (s.operational != sc_asda_state[0].operational)printf("sc_asda_state[0]: %soperational.\n",s.operational ? "" : "Not ");sc_asda_state[0] = s;ec_slave_config_state_t s1;ecrt_slave_config_state(sc_asda[1],&s1);if (s1.al_state != sc_asda_state[1].al_state)printf("sc_asda_state[1]: State 0x%02X.\n", s1.al_state);if (s1.online != sc_asda_state[1].online)printf("sc_asda_state[1]: %s.\n", s1.online ? "online" : "offline");if (s1.operational != sc_asda_state[1].operational)printf("sc_asda_state[1]: %soperational.\n",s1.operational ? "" : "Not ");sc_asda_state[1] = s1;ec_slave_config_state_t s2;ecrt_slave_config_state(sc_asda[2],&s2);if (s2.al_state != sc_asda_state[2].al_state)printf("sc_asda_state[2]: State 0x%02X.\n", s2.al_state);if (s2.online != sc_asda_state[2].online)printf("sc_asda_state[2]: %s.\n", s2.online ? "online" : "offline");if (s2.operational != sc_asda_state[2].operational)printf("sc_asda_state[2]: %soperational.\n",s2.operational ? "" : "Not ");sc_asda_state[2] = s2;
}
/****************************************************************************/
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_asda[0], 0x0300, 1000000, 0, 0, 0);ecrt_slave_config_dc(sc_asda[1], 0x0300, 1000000, 0, 0, 0);ecrt_slave_config_dc(sc_asda[2], 0x0300, 1000000, 0, 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;//处于刚开机(需要等待其他操作完成），返回等待下次周期if(gSysRunning.m_gWorkStatus == SYS_WORKING_POWER_ON)return ;static int cycle_counter = 0;cycle_counter++;if(cycle_counter >= 90*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;for(int i=0;i<3;i++){if(sc_asda_state[i].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");}}}break;case SYS_WORKING_OP_MODE:{ecstate++;//使能伺服if(ecstate <= 16){for(int i=0;i<3;i++){switch (ecstate){case 1:EC_WRITE_U16(domainOutput_pd + cntlwd[i], 0x80);       break;case 7:curpos = EC_READ_S32(domainInput_pd + actpos[i]);       EC_WRITE_S32(domainOutput_pd + ipData[i], EC_READ_S32(domainInput_pd + actpos[i])); printf("x@rtITP >>> Axis %d current position = %d\n", i, curpos);break;case 9:EC_WRITE_U16(domainOutput_pd + cntlwd[i], 0x06);break;case 11:EC_WRITE_U16(domainOutput_pd + cntlwd[i], 0x07);break;case 13:EC_WRITE_U16(domainOutput_pd + cntlwd[i], 0xF);break;}}}else {int tmp  = true;for(int i=0;i<3;i++){if((EC_READ_U16(domainInput_pd + status[i]) & (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:{if (!(cycle_counter % 1000)) {printf("curpos = %d\t",curpos);printf("asda0 actpos... %d\t",EC_READ_S32(domainInput_pd + actpos[0]));printf("asda1 actpos... %d\t",EC_READ_S32(domainInput_pd + actpos[1]));printf("asda2 actpos... %d\n",EC_READ_S32(domainInput_pd + actpos[2]));}curpos += 100;for(int i=0;i<3;i++){EC_WRITE_S32(domainOutput_pd + ipData[i], 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主站控制3个台达asdaa2伺服转圈圈的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/428178。

相关文章

用js控制视频播放进度基本示例代码

用js控制视频播放进度基本示例代码

《用js控制视频播放进度基本示例代码》写前端的时候,很多的时候是需要支持要网页视频播放的功能,下面这篇文章主要给大家介绍了关于用js控制视频播放进度的相关资料,文中通过代码介绍的非常详细,需要的朋友可... 目录前言html部分:JavaScript部分:注意：总结前言在javascript中控制视频播放
阅读更多...
Python异步编程中asyncio.gather的并发控制详解

Python异步编程中asyncio.gather的并发控制详解

《Python异步编程中asyncio.gather的并发控制详解》在Python异步编程生态中,asyncio.gather是并发任务调度的核心工具,本文将通过实际场景和代码示例,展示如何结合信号量... 目录一、asyncio.gather的原始行为解析二、信号量控制法：给并发装上"节流阀"三、进阶控制
阅读更多...
使用DrissionPage控制360浏览器的完美解决方案

使用DrissionPage控制360浏览器的完美解决方案

《使用DrissionPage控制360浏览器的完美解决方案》在网页自动化领域,经常遇到需要保持登录状态、保留Cookie等场景,今天要分享的方案可以完美解决这个问题：使用DrissionPage直接... 目录完整代码引言为什么要使用已有用户数据？核心代码实现1. 导入必要模块2. 关键配置（重点！）3.
阅读更多...
SpringSecurity 认证、注销、权限控制功能(注销、记住密码、自定义登入页)

SpringSecurity 认证、注销、权限控制功能(注销、记住密码、自定义登入页)

《SpringSecurity认证、注销、权限控制功能(注销、记住密码、自定义登入页)》SpringSecurity是一个强大的Java框架,用于保护应用程序的安全性,它提供了一套全面的安全解决方案... 目录简介认识Spring Security“认证”（Authentication）“授权” （Auth
阅读更多...
python之流程控制语句match-case详解

python之流程控制语句match-case详解

《python之流程控制语句match-case详解》：本文主要介绍python之流程控制语句match-case使用,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐... 目录match-case 语法详解与实战一、基础值匹配（类似 switch-case）二、数据结构解构匹
阅读更多...
Spring Security注解方式权限控制过程

Spring Security注解方式权限控制过程

《SpringSecurity注解方式权限控制过程》：本文主要介绍SpringSecurity注解方式权限控制过程,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录一、摘要二、实现步骤2.1 在配置类中添加权限注解的支持2.2 创建Controller类2.3 Us
阅读更多...
Python中如何控制小数点精度与对齐方式

Python中如何控制小数点精度与对齐方式

《Python中如何控制小数点精度与对齐方式》在Python编程中,数据输出格式化是一个常见的需求,尤其是在涉及到小数点精度和对齐方式时,下面小编就来为大家介绍一下如何在Python中实现这些功能吧... 目录一、控制小数点精度1. 使用 round() 函数2. 使用字符串格式化二、控制对齐方式1. 使用
阅读更多...
Springboot控制反转与Bean对象的方法

Springboot控制反转与Bean对象的方法

《Springboot控制反转与Bean对象的方法》文章介绍了SpringBoot中的控制反转（IoC）概念,描述了IoC容器如何管理Bean的生命周期和依赖关系,它详细讲解了Bean的注册过程,包括... 目录1 控制反转1.1 什么是控制反转1.2 SpringBoot中的控制反转2 Ioc容器对Bea
阅读更多...
浅析如何使用Swagger生成带权限控制的API文档

浅析如何使用Swagger生成带权限控制的API文档

《浅析如何使用Swagger生成带权限控制的API文档》当涉及到权限控制时,如何生成既安全又详细的API文档就成了一个关键问题,所以这篇文章小编就来和大家好好聊聊如何用Swagger来生成带有... 目录准备工作配置 Swagger权限控制给 API 加上权限注解查看文档注意事项在咱们的开发工作里，API
阅读更多...
Spring IOC控制反转的实现解析

Spring IOC控制反转的实现解析

《SpringIOC控制反转的实现解析》：本文主要介绍SpringIOC控制反转的实现,IOC是Spring的核心思想之一,它通过将对象的创建、依赖注入和生命周期管理交给容器来实现解耦,使开发者... 目录1. IOC的基本概念1.1 什么是IOC1.2 IOC与DI的关系2. IOC的设计目标3. IOC
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2