今天，正运动小助手给大家分享一下如何通过Qt实现EtherCAT总线初始化与轴调试。
运动控制实时内核MotionRT750
MotionRT750是正运动技术自主研发的首款基于x86架构、可在Windows或Linux系统下裸跑CPU的强实时运动控制内核。
该方案采用CPU内核独占技术，实现裸机级的强实时运动控制。它将 核心运动控制、机器人算法、数控（CNC）及机器视觉 等强实时任务集中运行在1-2个专用CPU核上。与此同时，其余CPU核则专注于处理Windows/Linux相关的非实时任务。
此外，该方案集成了MotionRT750 Runtime实时层与操作系统非实时层，并利用高速共享内存进行数据交互，显著提升运动控制与上层应用间的通信效率及函数执行速度，最终实现更稳定、更高效的智能装备控制，确保运动控制任务的绝对实时性与系统稳定性，特别适用于半导体、电子装备等高速、高精度应用场合。
强实时运动控制卡XPCIE1032H
支持4-64轴运动控制可选。
16个数字输入，其中8路高速输入，4路高速锁存输入，2路单端编码器反馈。
16个数字输出，其中16路高速PSO、4路PWM输出和4路单端脉冲输出。
视觉飞拍、精密点胶和激光能量控制。
点位、凸轮、直线、圆弧、连续轨迹以及50+机械手模型算法。
EtherCAT轴和 脉冲轴可以混合使用。
强实时运动控制卡XPCIE2032H
支持4-128轴运动控制可选。
2路EtherCAT独立主站，可125us 32轴，支持EtherCAT冗余功能。
可选配16+16输入，16+16路输出，含8路高速输入和16路高速输出，其中4路单端脉冲输出和16路硬件位置比较输出PSO。
支持直线插补、任意空间圆弧插补、螺旋插补、连续插补，电子凸轮、 电子齿轮 、位置锁存、同步跟随、 虚拟轴等功能。
支持RtBasic多文件多任务编程。
多种程序加密手段，保护客户的知识产权。
强实时运动控制卡XPCIE6032H
支持4-254轴运动控制可选。
6路EtherCAT独立主站，EtherCAT周期125us~1ms，每路主站可自由配置，支持EtherCAT冗余功能。
可选配16输入16输出，含8路高速输入和16路高速输出，其中4路单端脉冲输出。
支持直线插补、任意空间圆弧插补、 螺旋插补 、连续插补，电子凸轮、 电子齿轮、位置锁存、同步跟随、虚拟轴等功能。
支持RTBasic多文件多任务编程。
多种程序加密手段，保护客户的知识产权。
基于Linux的强实时EtherCAT运动控制器ZMC600M
ZMC600M系列高性能多轴运动控制器是一款EtherCAT总线立式运动控制器，控制器本身最多支持6-32轴的复杂的控制需求。
ZMC600M是基于 “ARM架构 + 纯国产Linux运动控制实时内核MotionRT750 + EtherCAT” 的总线型运动控制器，打破平台壁垒，实现了嵌入式系统与运动控制和实时内核技术的无缝衔接。
ZMC600M硬件特性：
内置Linux系统，搭载强实时内核MotionRT750；
可选6-32轴运动控制，EtherCAT总线最快通讯周期可达250μs；
24路高速IO输入，2路高速锁存，3路单端编码器接口；
16路高速IO输出，4路PWM/PSO，4路单端脉冲轴接口；
2路AD，2路DA（0-10V，12bit）；
支持EtherNET、CAN、RS232、RS485通讯；
1路MPG手轮接口；
支持掉电存储；
特殊型号支持5V/24V推挽输出功能；
支持多文件多任务编程，可支持PC程序和控制器内置程序同时工作。
软硬件全国产运动控制器ZMC432HG
EtherCAT运动控制器ZMC432HG是正运动推出的一款 软硬件全国产自主可控控制器， 专为满足客户在信息安全与供应链安全需求而开发的国产运动控制平台，能够适配大多数工业控制协同工业互联网应用需求。
ZMC432HG支持正运动远程显示、网络组态显示，可实时监控和调整参数配置，也可根据现场需求灵活组网，提升系统部署的适配性与扩展性。
ZMC432HG硬件特性：
多支持6-32轴运动控制；
EtherCAT总线最快1ms的刷新周期；
6路10MHz高速差分脉冲输出，总线轴、脉冲轴可混合插补；
2路模拟量输入（AD），2个模拟量输出（DA）；
4路通用IO输入，4路高速输入，支持高速锁存；
12路通用IO输出，4路高速输出，支持PWM和硬件比较输出功能；
轴正负限位信号/原点信号可以随意配置为任意通用数字输入口；
EtherCAT接口、RS232串口、RS485串口、以太网接口、CAN接口；
支持掉电检测、掉电存储，多种程序加密方式，能够有效防止系统故障，保护项目工程文件数据，提高系统的可靠性。
RTSys总线初始化与轴调试
1.连接控制器。 如果是通过其他电脑连接调试就通过 IP连接，确保对应电脑网口和控制器处于相同网段 （默认IP：192.168.0.11） ，卡装到本机就可以直接通过Local连接。
2.连接后点击“文件”→“新建工程”，工程建好后再点“文件”→“新建”文件 （选择basic文件类型）。
3.新建完成后找到RTSys右上角“帮助文档”，点击下拉选择“RTBasic帮助”找到 【第二十三章简易例程里面的EtherCAT初始化程序】 复制粘贴到刚刚新建的bas文件内。
4 .将驱动器或者模块的xml文件增加到配置文件，还要注意红色框选区域变量，要根据实际情况修改赋值，本地脉冲轴与总线轴轴号不能冲突，然后设置bas任务号，下载初始化程序进行初始化与EtherCAT驱动器或者其他EtherCAT模块进行通讯，下载后可根据命令与输出的输出信息进行查看，是否通讯成功，如果通讯不成功可以根据错误码排查问题。错误码也是在RTBasic帮助里面查看 （第二十四章附录一）。
5.通过工具里面的手动运动控制轴进行点动，点动之前要先设置好轴参数，避免看不到运动变化或者变化太大造成危险。
①线性运动建议将“ 脉冲当量”设置为机台运动1mm所需的脉冲数， 若按此推荐设置，“速度”的单位为mm/s，“加速度”和“减速度”的单位则为mm/s²。 脉冲当量 = 电机转一圈脉冲数 * 减速比/丝杆导程。
②旋转运动建议将“脉冲当量”设置为转1度所需的脉冲数， 若按此推荐设置，“速度”的单位为度/s，“加速度”和“减速度”的单位则为度/s²。 脉冲当量 = 电机转一圈脉冲数 * 减速比/360。
通常，【加速度】和【减速度】设置为速度的10倍。
若在调试过程中不确定机台运动1mm所需的脉冲数，可将【脉冲当量】设为1。
为安全起见，“速度”可先设为10，此时左右整体速度为1*10=10个脉冲每秒，“加速度”和“减速度”均设为1000。随后手动运动轴，观察轴是否运动及“反馈位置”是否有变化。
若手动运动时“反馈位置”数据变化但实际轴未动，可逐步将“脉冲当量”以10倍递增，每次调整后反复手动运动，检查轴是否运动。
“反馈位置”由编码器反馈，若其数据变化，轴必定在运动；若肉眼难以察觉，可能是机台运动较慢。
6.如果有IO或者模拟量扩展模块，初始化后也可以通过RTSys菜单栏找到“控制器”→“控制器状态”→“槽位0节点”，可以看到所有节点对应的IO或模拟量起始地址。可以在总线初始化完成后观看此界面确认IO或模拟量起始地址映射正常。
基于Qt实现EtherCAT总线初始化
例程基于 Windows QT编写 ， 主要实现功能： 连接M otionRT750 、建立M otionRT750 与驱动器或者扩展模块EtherCAT 连接、实现点动、寸动、原点限位映射、回零等。
一.PC函数手册获取
PC 函数手册可在光盘资料获取，具体路径如下：“ 00 光盘资料 /03 编程手册 /03ZMotion PC 函数库编程手册” 。
二.Qt项目创建
1. 打开 Qt 参考如下步骤创建 Qt 项目。
2.把厂商提供的zmotion.d1l、zmotion.lib、zmotion.h、zmcaux.cpp、zmcaux.h库文件拷贝到新建的 Qt项目下 （Linux系统使用1ibzmotion.so的动态库）。
3.添加外部库【选择项目后右键】→【添加库】→【外部库】。
4.引入头文件并定义控制器的句柄。
Qt调试的三种方式
方式一：
直接沿用上面RTSys 下载到ROM的初始化文件，可以直接设置轴参数后控制。
方式二：
1.将上面 RTSys中的工程项目生成ZAR文件。
2.Qt例程连接到控制器或者控制卡。
3. 在 Qt中通过 ZAux_ZarDown 来下载运行。
具体 ZAR调用代码如下。
QStringMainWindow::findZarFile()const { QDirdir(QCoreApplication::applicationDirPath()); for(inti=0;i< 6; ++i) { const QString path = dir.filePath(QStringLiteral("ZAR/Init.zar")); if (QFileInfo::exists(path)) return QDir::toNativeSeparators(path); if (!dir.cdUp()) break; } return QString(); } void MainWindow::myInit(int localAxisId, int localAxisNum, int driveAxisStart, int nodeNum) { // 可选下载 Init.zar → 预扫描 → 读节点 IO → 执行 EcatInit const QString zar = findZarFile(); if (!zar.isEmpty()) { const QByteArray path = zar.toLocal8Bit(); ZAux_ZarDown(m_handle, path.constData(), 0); emit logArrived(QStringLiteral("ZAR文件下载成功！！！"), QColor(184, 134, 11)); } }
4.调用后看命令与输出提示，如果报错误码跟RTSys调试一样，找到“ PC 函数库编程手册 ”查看第十四章“14.2指令返回值详细说明”。
5.通讯后设置好轴参数验证能不能点动控制 、寸动、回零等，动作之前要先设置好轴参数，避免看不到运动变化或者变化太大造成危险。
①线性运动建议将“脉冲当量”设置为机台运动1mm所需的脉冲数， 若按此推荐设置，“速度”的单位为mm/s，“加速度”和“减速度”的单位则为mm/s²， 脉冲当量 = 电机转一圈脉冲数 * 减速比/丝杆导程。
②旋转运动建议将“脉冲当量”设置为转1度所需的脉冲数， 若按此推荐设置，“速度”的单位为度/s，“加速度”和“减速度”的单位则为度/s²， 脉冲当量 = 电机转一圈脉冲数 * 减速比/360。
通常，【加速度】和【减速度】设置为速度的10倍。
若在调试过程中不确定机台运动1mm所需的脉冲数，可将【脉冲当量】设为1。
为安全起见，“速度”可先设为10，此时左右整体速度为1*10=10个脉冲每秒，“加速度”和“减速度”均设为1000。随后手动运动轴，观察轴是否运动及“反馈位置”是否有变化。
若手动运动时“反馈位置”数据变化但实际轴未动，可逐步将“脉冲当量”以10倍递增，每次调整后反复手动运动，检查轴是否运动。
“反馈位置”由编码器反馈，若其数据变化，轴必定在运动；若肉眼难以察觉，可能是机台运动较慢。
回零测试线选择回零轴号，配置轴的正限位信号、负限位信号及原点信号后，点击“配置轴IO”。若轴状态显示“30h”，则勾选：“反转”，并再次点击“配置轴IO”。
配置速度、爬行速度及回零模式后，点击“启动回零”。若出现方向错误或速度过快的情况，请立即点击“紧急停止”，并重新进行调试。
方式三：
直接在 Qt中写初始化，这种方式写的比较多，还需要理解初始化，快速验证不建议用。
具体流程如下：
总线初始化代码如下：
#include"ecatinit.h" #include#include#include#include//移植自C#ZMotionECAT.cs。 //初始化由上位机直接下发指令完成，不是靠下载Init.zar。 //API说明参考《ZMotionPC函数库编程手册》： //ZAux_Execute缓冲方式执行BASIC在线命令，可取返回字符串 //ZMC_ExecuteGetReceive取上一条还没读完的应答 //ZAux_Direct_Rapidstop急停，imode=2取消当前运动和缓冲 //ZAux_GetSysSpecification读控制器轴数、IO规格 //ZAux_Direct_SetAxisAddress/SetAtype/SetAxisEnable轴地址、轴类型、使能 //ZAux_BusCmd_SDOWrite写从站对象字典 //ZAux_Direct_SetFwdIn/SetRevIn/SetDatumIn/SetInvertIn限位、原点及取反 EcatInitInfoSet::EcatInitInfoSet() { InitStructFlag=1;//1=使用下面这组默认参数；界面传入时会改成0 LocalAxisId=0;//本地脉冲轴起始轴号，默认0 LocalAxisNum=0;//本地脉冲轴数量，默认0表示不用本地轴 DriveAxisStart=0;//总线驱动轴起始轴号，第1个驱动轴映射到此轴 DriveAxisNum=-1;//期望总线轴数，-1表示不校验实际轴数 DriveIoStara=256;//驱动器IO映射到控制器后的起始IO号 DriveIoSpa=16;//每个驱动器占用的IO点数 DriveEnable=1;//1=初始化成功后自动使能，0=不使能 EcatNodeNum=-1;//期望从站个数，-1表示不校验 SysClockMode=1;//1=打开DC同步时钟优化 memset(DrivePdoMode,0,sizeof(DrivePdoMode));//PDO模式先清零，默认分支里再写成12 memset(NodeIoId,0,sizeof(NodeIoId));//各节点数字IO起始地址 memset(NodeAIoId,0,sizeof(NodeAIoId));//各节点模拟量起始地址 memset(DcOffsetFlag,0,sizeof(DcOffsetFlag));//哪些节点需要写DC偏移 memset(DcOffsetTime,0,sizeof(DcOffsetTime));//各节点DC偏移时间，单位与例程一致 } voidMyDelayMs(intdelayTime,int*pOutTime) { Sleep(static_cast(delayTime));//阻塞等待，单位毫秒 if(pOutTime) *pOutTime-=delayTime;//同时扣掉总超时，避免扫描无限等 } staticboolisMinusOneReply(constchar*s) { //SLOT_SCAN/SLOT_START成功时，控制器在线命令返回字符串"-1" if(!s||!s[0]) returnfalse;//空串表示命令还没返回 while(*s==''||*s=='t') ++s;//跳过前导空白 if(!(s[0]=='-'&&s[1]=='1')) returnfalse;//不是-1 constcharc=s[2]; returnc=='�'||c=='n'||c=='r';//后面只能是结束或换行}staticintexecCmd(ZMC_HANDLEhandle,constchar*cmd,char*resp,uint32len) { //ZAux_Execute：把BASIC命令放进指令缓冲执行，resp接收?查询的返回值 if(resp&&len>0) memset(resp,0,len);//先清空，避免上次应答残留 returnZAux_Execute(handle,cmd,resp,len);}staticintparseIntReply(constchar*resp) { if(!resp||!resp[0]) return0;//没有返回就当0 returnatoi(resp);//控制器数字应答转成整数 } intZAux_BusCmd_SlotStop(ZMC_HANDLEhandle,intSlotId) { charcmd[64];//拼好的SLOT_STOP命令 charrece[256];//这条命令的返回，停止本身一般没有有效数字 snprintf(cmd,sizeof(cmd),"SLOT_STOP(%d)",SlotId);//停止指定槽位总线，扫描前必须先停 returnexecCmd(handle,cmd,rece,sizeof(rece)); } intZAux_BusCmd_SlotScan(ZMC_HANDLEhandle,intSlotId,int*OutTime,std::string*AckBuff) { //扫描EtherCAT从站。返回1成功，ScanFailu(-6)失败，0表示超时仍无结果 intscanOkFlag=0;//0超时还没扫到，1扫描成功 intiresult=0;//累加ZAux_Execute的返回码，0表示调用没出错 uint32puiread=0;//ZMC_ExecuteGetReceive这次实际读到多少字节 uint8pbifExcuteDown=0;//1表示这条在线命令已经执行完 charzmcReceBuff[256];//异步应答，扫描慢时结果在这里 charreceBuff[1024];//ZAux_Execute立刻带回的字符串 charcmd[64];//拼出来的SLOT_SCAN命令 intecatScanOutTime=OutTime?*OutTime:0;//本函数自己倒计时，到0就不再等 snprintf(cmd,sizeof(cmd),"SLOT_SCAN(%d)?RETURN",SlotId);//扫描并立刻查询返回值 iresult+=execCmd(handle,cmd,receBuff,sizeof(receBuff));if(isMinusOneReply(receBuff)) return1;//立刻得到-1，扫描成功 if(receBuff[0]!='�') { if(AckBuff) *AckBuff=receBuff;//把失败文本带回界面日志 returnstatic_cast(ScanFailu); } MyDelayMs(500,OutTime);//同步应答为空，等控制器后台扫完 ecatScanOutTime-=500; while(ecatScanOutTime>0) { memset(zmcReceBuff,0,sizeof(zmcReceBuff)); puiread=0; pbifExcuteDown=0; //手册：ZMC_ExecuteGetReceive读取还没取走的命令应答，没有新数据时缓冲为0 iresult+=ZMC_ExecuteGetReceive(handle,zmcReceBuff,sizeof(zmcReceBuff),&puiread,&pbifExcuteDown); if(zmcReceBuff[0]!=0) { //ASCII45,49,10就是字符"-1n" if(static_cast(zmcReceBuff[0])==45 &&static_cast(zmcReceBuff[1])==49 &&static_cast(zmcReceBuff[2])==10) { scanOkFlag=1; break; } scanOkFlag=static_cast(ScanFailu); if(AckBuff) *AckBuff=receBuff; break; } MyDelayMs(50,OutTime);//还没有应答，50ms后再读 ecatScanOutTime-=50; } (void)iresult; returnscanOkFlag; } intZAux_BusCmd_SlotStart(ZMC_HANDLEhandle,intSlotId,int*OutTime,std::string*AckBuff) { //启动总线。参数4=进入PreOP，参数8=进入OP。返回1成功，0失败 intstartOkFlag=0;//0启动失败或超时，1总线已进OP intiresult=0;//累加API返回码 charreceBuff[1024];//SLOT_START同步返回的字符串 charcmd[64];//拼出来的SLOT_START命令 uint32puiread=0;//异步读取到的字节数 uint8pbifExcuteDown=0;//1表示启动命令已经执行完 charzmcReceBuff[256];//启动较慢时，"-1"会晚一点出现在这里 intecatScanOutTime=OutTime?*OutTime:0;//等待进OP的剩余时间 MyDelayMs(100,OutTime);//停总线后稍等再启动 snprintf(cmd,sizeof(cmd),"SLOT_START(%d,4)",SlotId);//先切到PreOP iresult+=execCmd(handle,cmd,receBuff,256); MyDelayMs(1000,OutTime);//给从站进入PreOP的时间 snprintf(cmd,sizeof(cmd),"SLOT_START(%d,8)?Return",SlotId);//再切到OP，并查询结果 iresult+=execCmd(handle,cmd,receBuff,256); if(isMinusOneReply(receBuff)) return1;//立刻成功 if(receBuff[0]!='�') { if(AckBuff) *AckBuff=receBuff;//非-1的文本是启动失败原因 return0; } MyDelayMs(500,OutTime); ecatScanOutTime-=500; while(ecatScanOutTime>0) { memset(zmcReceBuff,0,sizeof(zmcReceBuff)); puiread=0; pbifExcuteDown=0; iresult+=ZMC_ExecuteGetReceive(handle,zmcReceBuff,sizeof(zmcReceBuff),&puiread,&pbifExcuteDown); if(zmcReceBuff[0]!=0) { if(static_cast(zmcReceBuff[0])==45 &&static_cast(zmcReceBuff[1])==49 &&static_cast(zmcReceBuff[2])==10) { startOkFlag=1;//异步读到"-1n" break; } startOkFlag=0; if(AckBuff) *AckBuff=receBuff; break; } MyDelayMs(50,OutTime); ecatScanOutTime-=50; } (void)iresult; returnstartOkFlag; } intZAux_BusCmd_EcatInit(ZMC_HANDLEhandle,intSlotId,EcatInitInfoSet*EcatInfo,intApiOutTime,std::string*AckBuff) { if(!EcatInfo) returnstatic_cast(ScanFailu);//没有参数结构不能初始化//总流程：停轴并清映射→映射本地脉冲轴→设DC时钟 //→第一次扫描写DC偏移→第二次扫描映射总线轴/PDO/IO //→SLOT_START→写控制字、看门狗、使能 intiresult=0;//各API返回码累加，非0表示中途有调用失败 intoutTime=ApiOutTime;//整段初始化允许的总耗时，界面传入约30秒 intidleOutTime=100;//等单轴停下来的时间，毫秒 intscanNodeNum=0;//本次扫描到的从站个数 charreceBuff[256];//在线命令返回缓冲 charcmdBuff[256];//拼出来的BASIC命令 intdriveVender=0;//NODE_INFOsel=0，厂商ID intdriveDevice=0;//NODE_INFOsel=1，设备ID intdriveAlias=0;//NODE_INFOsel=3，别名，当前只读出备用//是否使用结构体里的默认Ecat初始化参数。界面把Flag设为0，走自己填的参数 if(EcatInfo->InitStructFlag==1) { //1.本地轴：指定脉冲轴从哪一号开始、一共几根 EcatInfo->LocalAxisId=0;//本地轴起始ID，使用时从0开始 EcatInfo->LocalAxisNum=0;//本地轴数量，0表示没有脉冲轴 //2.驱动轴：指定总线轴起始号、轴数，以及IO怎么铺到控制器 EcatInfo->DriveAxisStart=0;//第1根总线轴映射到的控制器轴号 EcatInfo->DriveAxisNum=-1;//-1不检查扫描到的轴数是否和设定一致 EcatInfo->DriveIoStara=256;//驱动器IO映射后的起始号，避开控制器本地IO EcatInfo->DriveIoSpa=16;//每个驱动器占16点IOfor(inti=0;i< 128; ++i) { // PDO 模式，对应 BASIC 的 DRIVE_PROFILE，12 为带 IO 的位置模式，见 RTBasic 手册 EcatInfo->DrivePdoMode[i]=12; } EcatInfo->DriveEnable=1;//1初始化结束后自动AXIS_ENABLE，0不自动使能 //3.从站数量和节点IO/AIO扩展 EcatInfo->EcatNodeNum=-1;//-1不检查从站个数 for(inti=0;i< 128; ++i) { EcatInfo->NodeIoId[i]=0;//节点数字IO起始，0表示还没分配 EcatInfo->NodeAIoId[i]=0;//节点模拟量起始 } //4.DC同步时钟和DC偏移 EcatInfo->SysClockMode=1;//1打开时钟优化 for(inti=0;i< 128; ++i) { EcatInfo->DcOffsetFlag[i]=0;//0该节点不改DC偏移 EcatInfo->DcOffsetTime[i]=0;//偏移时间 } EcatInfo->DcOffsetFlag[0]=1;//默认只给第0个节点写偏移 EcatInfo->DcOffsetTime[0]=0.5f;//0.5个伺服周期 } constintelmoVender=0x9a;//Elmo厂商ID constintelmoDevice=0x30924;//Elmo设备ID，这类从站要单独配PDO //手册：ZAux_Direct_Rapidstop，imode=2立即停止当前运动和运动缓冲 iresult=ZAux_Direct_Rapidstop(handle,2); uint16virtualAxisNum=0;//控制器虚拟轴总数，后面按这个数清轴 uint8motionAxisNum[1]={0};//实轴数量，手册输出参数，这里不用 uint8ioNum[4]={0,0,0,0};//IN/OUT/AD/DA规格，这里不用//手册：读取系统规格，得到最大轴数，避免只清前几轴 ZAux_GetSysSpecification(handle,&virtualAxisNum,motionAxisNum,ioNum); for(inti=0;i< virtualAxisNum; ++i) { // 轴地址清 0，断开原来的总线或脉冲映射 iresult += ZAux_Direct_SetAxisAddress(handle, i, 0); // 先关使能，防止改类型时驱动器还在出力 iresult += ZAux_Direct_SetAxisEnable(handle, i, 0); // ATYPE=0 表示虚拟轴，等于先把轴类型还原 iresult += ZAux_Direct_SetAtype(handle, i, 0); int idle = 0; // -1 表示该轴已经空闲 idleOutTime = 100; while (idleOutTime >0) { //手册：GetIfIdle，-1空闲，0运动中 iresult+=ZAux_Direct_GetIfIdle(handle,i,&idle); if(idle==-1) break; MyDelayMs(10,&outTime); idleOutTime-=10; } } for(inti=0;i< EcatInfo->LocalAxisNum;++i) { //高16位-1（数值-65536）表示本地脉冲轴，低位是脉冲通道号 iresult+=ZAux_Direct_SetAxisAddress(handle,EcatInfo->LocalAxisId+i,-65536+i); iresult+=ZAux_Direct_SetAtype(handle,EcatInfo->LocalAxisId+i,1);//ATYPE=1脉冲方向轴 } if(EcatInfo->SysClockMode==1) //SET_BIT(7)打开伺服同步时钟优化，给DC模式用 iresult+=execCmd(handle,"SYSTEM_ZSET=SET_BIT(7,SYSTEM_ZSET)",receBuff,sizeof(receBuff)); else iresult+=execCmd(handle,"SYSTEM_ZSET=CLEAR_BIT(7,SYSTEM_ZSET)",receBuff,sizeof(receBuff)); if(iresult!=0) returniresult;//前面清轴或设时钟已经失败，不再扫描 //第一次扫描只为了写DC偏移，最多试3次 for(intii=0;ii< 3; ++ii) { iresult += ZAux_BusCmd_SlotStop(handle, SlotId); // 扫描前先停槽 MyDelayMs(200, &outTime); if (ZAux_BusCmd_SlotScan(handle, SlotId, &outTime, AckBuff) == 1) { snprintf(cmdBuff, sizeof(cmdBuff), "?NODE_COUNT(%d)", SlotId); // 查询该槽从站个数 iresult += execCmd(handle, cmdBuff, receBuff, sizeof(receBuff)); scanNodeNum = parseIntReply(receBuff); for (int j = 0; j < scanNodeNum; ++j) { if (EcatInfo->DcOffsetFlag[j]!=1) continue;//该节点不需要改DC snprintf(cmdBuff,sizeof(cmdBuff),"?NODE_INFO(%d,%d,0)",SlotId,j);//sel=0厂商ID iresult+=execCmd(handle,cmdBuff,receBuff,sizeof(receBuff)); driveVender=parseIntReply(receBuff); snprintf(cmdBuff,sizeof(cmdBuff),"?NODE_INFO(%d,%d,1)",SlotId,j);//sel=1设备ID iresult+=execCmd(handle,cmdBuff,receBuff,sizeof(receBuff)); driveDevice=parseIntReply(receBuff); //ZML_INFO(19)是DC偏移，单位与伺服周期相关 snprintf(cmdBuff,sizeof(cmdBuff), "ZML_INFO(19,%d,%d)=SERVO_PERIOD*%g*1000", driveVender,driveDevice,static_cast(EcatInfo->DcOffsetTime[j])); iresult+=execCmd(handle,cmdBuff,receBuff,sizeof(receBuff)); } break;//这一轮扫描成功就不用再重试 } } //第二次扫描：核对数量，映射轴、PDO、节点IO，然后启动总线 for(intii=0;ii< 3; ++ii) { iresult += ZAux_BusCmd_SlotStop(handle, SlotId); MyDelayMs(200, &outTime); const int scanRet = ZAux_BusCmd_SlotScan(handle, SlotId, &outTime, AckBuff); if (scanRet == 1) { snprintf(cmdBuff, sizeof(cmdBuff), "?NODE_COUNT(%d)", SlotId); iresult += execCmd(handle, cmdBuff, receBuff, sizeof(receBuff)); scanNodeNum = parseIntReply(receBuff); if (EcatInfo->EcatNodeNum>=0) { if(scanNodeNum!=EcatInfo->EcatNodeNum) returnstatic_cast(WrongNodeNum);//实际从站数和界面填写的不一致 } intbusAxisNum=0;//所有从站的轴加在一起，用来和DriveAxisNum比较 intnodeAxisNum=0;//当前这一个从站自己有几根轴 if(EcatInfo->DriveAxisNum>=0) { for(intj=0;j< scanNodeNum; ++j) { snprintf(cmdBuff, sizeof(cmdBuff), "?NODE_AXIS_COUNT(%d,%d)", SlotId, j); iresult += execCmd(handle, cmdBuff, receBuff, sizeof(receBuff)); nodeAxisNum = parseIntReply(receBuff); busAxisNum += nodeAxisNum; } if (busAxisNum != EcatInfo->DriveAxisNum) returnstatic_cast(WrongAxisNum);//实际轴数和设定不一致 } busAxisNum=0;//重新从0计，后面按这个序号映射轴 intservoPeriod=0;//伺服周期SERVO_PERIOD，单位us，用来核对DC偏移 iresult+=execCmd(handle,"?SERVO_PERIOD",receBuff,sizeof(receBuff)); servoPeriod=parseIntReply(receBuff); for(inti=0;i< scanNodeNum; ++i) { snprintf(cmdBuff, sizeof(cmdBuff), "?NODE_AXIS_COUNT(%d,%d)", SlotId, i); iresult += execCmd(handle, cmdBuff, receBuff, sizeof(receBuff)); nodeAxisNum = parseIntReply(receBuff); snprintf(cmdBuff, sizeof(cmdBuff), "?NODE_INFO(%d,%d, 0)", SlotId, i); iresult += execCmd(handle, cmdBuff, receBuff, sizeof(receBuff)); driveVender = parseIntReply(receBuff); snprintf(cmdBuff, sizeof(cmdBuff), "?NODE_INFO(%d,%d, 1)", SlotId, i); iresult += execCmd(handle, cmdBuff, receBuff, sizeof(receBuff)); driveDevice = parseIntReply(receBuff); snprintf(cmdBuff, sizeof(cmdBuff), "?NODE_INFO(%d,%d, 3)", SlotId, i); // sel=3 别名 iresult += execCmd(handle, cmdBuff, receBuff, sizeof(receBuff)); driveAlias = parseIntReply(receBuff); (void)driveAlias; if (EcatInfo->DcOffsetFlag[i]==1) { intzmlInfo=0;//ZML_INFO(19)里为这个厂商/设备记下的DC偏移 intnodeInfo=0;//NODE_INFOsel=19，这个节点当前实际的DC偏移 snprintf(cmdBuff,sizeof(cmdBuff),"?ZML_INFO(19,%d,%d)",driveVender,driveDevice); iresult+=execCmd(handle,cmdBuff,receBuff,sizeof(receBuff)); zmlInfo=parseIntReply(receBuff); snprintf(cmdBuff,sizeof(cmdBuff),"?NODE_INFO(%d,%d,19)",SlotId,i);//sel=19DC偏移 iresult+=execCmd(handle,cmdBuff,receBuff,sizeof(receBuff)); nodeInfo=parseIntReply(receBuff); //偏差超过5，或节点值和配置值不同，认为DC没写成功 if(((servoPeriod*1000*EcatInfo->DcOffsetTime[i]-zmlInfo)>5)||(zmlInfo!=nodeInfo)) returnstatic_cast(DcShiftSetFailu); } for(intj=0;j< nodeAxisNum; ++j) { // 总线轴地址从 1 开始编，ATYPE=65 表示 EtherCAT 周期位置轴 iresult += ZAux_Direct_SetAxisAddress(handle, EcatInfo->DriveAxisStart+busAxisNum,busAxisNum+1); iresult+=ZAux_Direct_SetAtype(handle,EcatInfo->DriveAxisStart+busAxisNum,65); if((driveDevice==elmoDevice)&&(driveVender==elmoVender)) { //Elmo：关掉时钟优化，PDO改成自定义，再写对象字典 iresult+=execCmd(handle,"SYSTEM_ZSET=CLEAR_BIT(7,SYSTEM_ZSET)",receBuff,sizeof(receBuff)); snprintf(cmdBuff,sizeof(cmdBuff),"DRIVE_PROFILE(%d)=-1",EcatInfo->DriveAxisStart+busAxisNum); iresult+=execCmd(handle,cmdBuff,receBuff,sizeof(receBuff));//-1表示不使用预设PDO模板 snprintf(cmdBuff,sizeof(cmdBuff),"NODE_PROFILE(%d,%d)=-1",SlotId,i); iresult+=execCmd(handle,cmdBuff,receBuff,sizeof(receBuff)); //手册：ZAux_BusCmd_SDOWrite(handle,槽,节点,索引,子索引,数据类型,值) //type5=UINT8，6=UINT16，7=UINT32 iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1c12,0,5,0);//清空RxPDO分配个数 iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1c1c,0,5,0); MyDelayMs(50,&outTime); iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1a07,0,5,0);//TxPDO映射先清个数 iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1a07,1,7,0x60410010);//状态字 iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1a07,2,7,0x60770010);//转矩实际值 iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1a07,3,7,0x60640020);//实际位置 iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1a07,4,7,0x60fd0020);//数字输入 iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1a07,5,7,0x60b90010); iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1a07,6,7,0x60ba0020); iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1a07,7,7,0x60bb0020); iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1a07,0,5,0x7);//TxPDO共7个对象 iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1607,0,5,0);//RxPDO映射先清个数 iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1607,1,7,0x60400010);//控制字 iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1607,2,7,0x60710010);//目标转矩 iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1607,3,7,0x60ff0020);//目标速度 iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1607,4,7,0x607a0020);//目标位置 iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1607,5,7,0x60b80010); iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1607,6,7,0x60720010); iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1607,7,7,0x60600008);//运行模式 iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1607,0,5,0x7);//RxPDO共7个对象 iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1c12,0,6,0x1607);//指定使用0x1607这组RxPDO iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1c12,1,5,1);//RxPDO分配个数=1 iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1c13,0,6,0x1a07);//指定使用0x1A07这组TxPDO iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x1c13,1,5,1); iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x6040,0,6,0);//控制字先写0 MyDelayMs(50,&outTime); iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x6040,0,6,7);//Shutdown/Switchon MyDelayMs(50,&outTime); iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x6040,0,6,7); MyDelayMs(50,&outTime); iresult+=ZAux_BusCmd_SDOWrite(handle,static_cast(SlotId),static_cast(i),0x6040,0,6,15);//Enableoperation } elseif((driveDevice==0x1ab0)&&(driveVender==0x41B)) { //正运动驱动器使用预设PDO模板0 snprintf(cmdBuff,sizeof(cmdBuff),"DRIVE_PROFILE(%d)=0",EcatInfo->DriveAxisStart+busAxisNum); iresult+=execCmd(handle,cmdBuff,receBuff,sizeof(receBuff)); } else { //其它驱动器用界面/默认的DRIVE_PROFILE snprintf(cmdBuff,sizeof(cmdBuff),"DRIVE_PROFILE(%d)=%d", EcatInfo->DriveAxisStart+busAxisNum,EcatInfo->DrivePdoMode[busAxisNum]); iresult+=execCmd(handle,cmdBuff,receBuff,sizeof(receBuff)); constintpdo=EcatInfo->DrivePdoMode[busAxisNum];//这根轴的DRIVE_PROFILE if(pdo==4||pdo==5||pdo==12) { //这几种PDO带驱动器IO，把负限位、正限位、原点映到连续3个输入并取反 constintstartIdTemp=EcatInfo->DriveIoStara+EcatInfo->DriveIoSpa*busAxisNum;//这根轴的IO起点 snprintf(cmdBuff,sizeof(cmdBuff),"DRIVE_IO(%d)=%d", EcatInfo->DriveAxisStart+busAxisNum,startIdTemp); iresult+=execCmd(handle,cmdBuff,receBuff,sizeof(receBuff)); iresult+=ZAux_Direct_SetRevIn(handle,EcatInfo->DriveAxisStart+busAxisNum,startIdTemp);//负限位 iresult+=ZAux_Direct_SetInvertIn(handle,startIdTemp,1);//该输入取反 iresult+=ZAux_Direct_SetFwdIn(handle,EcatInfo->DriveAxisStart+busAxisNum,startIdTemp+1);//正限位 iresult+=ZAux_Direct_SetInvertIn(handle,startIdTemp+1,1); iresult+=ZAux_Direct_SetDatumIn(handle,EcatInfo->DriveAxisStart+busAxisNum,startIdTemp+2);//原点 iresult+=ZAux_Direct_SetInvertIn(handle,startIdTemp+2,1); } elseif(pdo< 4) { // 不含 IO 的 PDO，如果以前映过驱动器 IO，就拆掉并取消取反 int tempVar = 0; // 读回来的原限位/原点输入号，用来判断要不要拆掉 iresult += ZAux_Direct_GetRevIn(handle, EcatInfo->DriveAxisStart+busAxisNum,&tempVar); if(tempVar>=EcatInfo->DriveIoStara) { iresult+=ZAux_Direct_SetRevIn(handle,EcatInfo->DriveAxisStart+busAxisNum,-1);//-1表示不映射 iresult+=ZAux_Direct_SetInvertIn(handle,tempVar,0); } iresult+=ZAux_Direct_GetFwdIn(handle,EcatInfo->DriveAxisStart+busAxisNum,&tempVar); if(tempVar>=EcatInfo->DriveIoStara) { iresult+=ZAux_Direct_SetFwdIn(handle,EcatInfo->DriveAxisStart+busAxisNum,-1); iresult+=ZAux_Direct_SetInvertIn(handle,tempVar,0); } iresult+=ZAux_Direct_GetDatumIn(handle,EcatInfo->DriveAxisStart+busAxisNum,&tempVar); if(tempVar>=EcatInfo->DriveIoStara) { iresult+=ZAux_Direct_SetDatumIn(handle,EcatInfo->DriveAxisStart+busAxisNum,-1); iresult+=ZAux_Direct_SetInvertIn(handle,tempVar,0); } } } snprintf(cmdBuff,sizeof(cmdBuff),"DISABLE_GROUP(%d)",EcatInfo->DriveAxisStart+busAxisNum); iresult+=execCmd(handle,cmdBuff,receBuff,sizeof(receBuff));//退出插补组，避免组运动占用这根轴busAxisNum+=1;//下一根总线轴 } if(EcatInfo->NodeIoId[i]>=32) { //数字IO起始号大于等于32才写入NODE_IO，小于32视为未使用 snprintf(cmdBuff,sizeof(cmdBuff),"NODE_IO(%d,%d)=%d",SlotId,i,EcatInfo->NodeIoId[i]); iresult+=execCmd(handle,cmdBuff,receBuff,sizeof(receBuff)); } if(EcatInfo->NodeAIoId[i]>0) { snprintf(cmdBuff,sizeof(cmdBuff),"NODE_AIO(%d,%d)=%d",SlotId,i,EcatInfo->NodeAIoId[i]); iresult+=execCmd(handle,cmdBuff,receBuff,sizeof(receBuff)); } if((driveDevice==0x1AB0)&&(driveVender==0x41B)) { //正运动扩展IO模块：按轴打开模块并使能相关对象 for(intk=0;k< nodeAxisNum; ++k) { ZAux_BusCmd_SDOWrite(handle, static_cast(SlotId), static_cast(i), static_cast(0x6011 + k * 0x800), 0, 5, 1); ZAux_BusCmd_SDOWrite(handle, static_cast(SlotId), static_cast(i), static_cast(0x6012 + k * 0x800), 0, 6, 0); } } } if (ZAux_BusCmd_SlotStart(handle, SlotId, &outTime, AckBuff) == 1) { MyDelayMs(3000, &outTime); // 等从站进 OP 并且驱动器上电稳定 for (int drivei = EcatInfo->DriveAxisStart;drivei< (EcatInfo->DriveAxisStart+busAxisNum);++drivei) { snprintf(cmdBuff,sizeof(cmdBuff),"DRIVE_CONTROLWORD(%d)=128",drivei);//Faultreset iresult+=execCmd(handle,cmdBuff,receBuff,sizeof(receBuff)); MyDelayMs(10,&outTime); snprintf(cmdBuff,sizeof(cmdBuff),"DRIVE_CONTROLWORD(%d)=6",drivei);//Shutdown iresult+=execCmd(handle,cmdBuff,receBuff,sizeof(receBuff)); MyDelayMs(10,&outTime); snprintf(cmdBuff,sizeof(cmdBuff),"DRIVE_CONTROLWORD(%d)=15",drivei);//Enableoperation iresult+=execCmd(handle,cmdBuff,receBuff,sizeof(receBuff)); MyDelayMs(10,&outTime); } MyDelayMs(10,&outTime); //手册：Single_Datum模式0，把当前点当作机械零点，清内部回零状态 ZAux_Direct_Single_Datum(handle,0,0); MyDelayMs(200,&outTime); iresult+=execCmd(handle,"WDOG=1",receBuff,sizeof(receBuff));//打开看门狗，总线掉线能被检测到 if(EcatInfo->DriveEnable==1) { //从最后一根轴往前使能，减轻同时上电的电流冲击 for(intdrivei=EcatInfo->DriveAxisStart+busAxisNum-1;drivei>=EcatInfo->DriveAxisStart;--drivei) { ZAux_Direct_SetAxisEnable(handle,drivei,1);//1=使能 MyDelayMs(10,&outTime); } } returniresult;//0表示整段初始化成功 } returnstatic_cast(EcatStartFailu);//扫描成功但SLOT_START失败 } if(scanRet==static_cast(ScanFailu)) returnstatic_cast(ScanFailu);//扫描命令本身报错，不再重试 } returnstatic_cast(NotScanNode);//三次都没扫到从站 }
通讯后重复上面第5小节，验证轴动作。
