1. 为什么我要自己封装轴控功能块1.1 从一次产线改造说起去年接手了一条包装产线的电控改造现场用的是汇川AM402 PLC配IS620N伺服驱动器走EtherCAT总线一共8个轴。项目初期我图省事每个轴都单独写了一套控制逻辑——使能、回零、点动、绝对定位、相对定位每个动作都从头写一遍。结果8个轴写下来程序里光是重复的梯形图就占了将近两千行调试的时候改一个参数要在8个地方同步修改漏掉一处就出问题。更头疼的是后面客户要加两个轴我又得把那一大坨逻辑复制粘贴两遍改完还得重新测一遍所有轴。那次之后我就下定决心必须把轴控逻辑封装成功能块。后来用ST语言在AM402上做了一套通用的轴控FB8个轴调用同一个功能块程序量直接砍到原来的三分之一加轴的时候只需要多调用一次、改一下轴号映射就行。这篇文章就把这套封装思路和完整实现过程拆开讲清楚包括IS620N的PDO配置、AM402的轴参数设置、功能块的接口设计和内部逻辑以及我在调试中踩过的那些坑。1.2 这篇文章适合谁看如果你正在用汇川AM402或者类似的CODESYS平台PLC控制IS620N伺服而且轴的数量超过3个那这套封装思路能帮你省下大量重复劳动。如果你刚开始接触PLC编程对功能块FB和ST语言还不太熟也没关系我会从最基础的概念讲起把每个参数为什么这么设、每段代码为什么这么写都解释清楚。即使你用的是其他品牌的PLC和伺服功能块封装的思路也是通用的只是具体接口和参数需要根据你的硬件调整。提示本文涉及的AM402和IS620N都是汇川的产品但功能块封装的思路不绑定品牌。如果你用的是其他支持CODESYS的PLC逻辑框架可以直接借鉴。1.3 封装前先想清楚三件事在动手写代码之前有三件事必须先想明白否则封装出来的功能块要么不好用要么根本没法复用。第一件事是接口设计。功能块对外暴露哪些输入、哪些输出这决定了它好不好用。输入太多调用的时候填参数就填半天输入太少灵活性又不够。我的经验是把输入分成两类一类是每个轴都不同的配置参数比如轴号、软限位、速度上限另一类是运行时的控制指令比如使能、回零、定位触发。输出则主要是状态反馈和故障信息。第二件事是状态机设计。伺服轴的控制本质上是一个状态机从初始化到使能从使能到就绪从就绪到运行运行完回到就绪出故障了要能复位。如果状态机设计得不好就会出现“使能了但没准备好就发定位指令”这种问题伺服直接报错。我见过不少项目就是因为状态机没理清楚调试的时候各种莫名其妙的报警。第三件事是异常处理。伺服驱动器会报各种故障——过流、过载、编码器异常、跟随误差过大等等。功能块必须能捕获这些故障并且提供明确的复位路径。如果异常处理没做好一旦伺服报警整个功能块就卡死了只能重启PLC。2. 硬件组态与IS620N的PDO配置2.1 AM402与IS620N的EtherCAT连接AM402是汇川基于CODESYS平台的中型PLC自带EtherCAT主站口。IS620N是汇川的EtherCAT总线型伺服驱动器支持CiA402协议。两者通过EtherCAT连接接线很简单PLC的EtherCAT口出来网线接到第一个IS620N的IN口再从第一个的OUT口接到第二个的IN口依次串下去。最后一个伺服的OUT口空着就行。这里有个细节要注意EtherCAT的拓扑是逻辑环物理上虽然是线型连接但数据会从最后一个节点绕回来。所以如果你中间某个节点断电了后面的节点也会通信中断。我在现场就遇到过因为一个伺服驱动器电源松动导致后面三个轴全部掉线的情况排查了半天才发现是接头问题。在CODESYS的设备树里添加AM402之后右键EtherCAT主站添加IS620N的ESI文件。汇川官网可以下载到IS620N的XML设备描述文件导入之后就能在设备树里看到IS620N的型号了。添加的时候要注意选择正确的固件版本不同版本的PDO映射可能不一样。2.2 PDO映射哪些数据必须映射PDO过程数据对象是EtherCAT通信中周期性传输的数据。IS620N的PDO分两种RxPDO是PLC发给伺服的控制字TxPDO是伺服反馈给PLC的状态字。CiA402协议定义了一套标准的控制字和状态字但具体映射哪些对象需要根据你的应用来定。对于基本的轴控功能我建议至少映射以下对象方向对象索引说明RxPDOControlword0x6040控制字控制伺服状态机RxPDOTarget Position0x607A目标位置RxPDOTarget Velocity0x60FF目标速度RxPDOModes of Operation0x6060运行模式TxPDOStatusword0x6041状态字反馈伺服状态TxPDOActual Position0x6064实际位置TxPDOActual Velocity0x606C实际速度TxPDOError Code0x603F故障码如果你需要用到力矩控制还要加上Target Torque0x6071和Actual Torque0x6077。如果要做位置比较输出比如飞拍还需要映射Touch Probe相关的对象。注意PDO映射不是越多越好。每增加一个对象通信数据量就增大一点。8个轴如果每个轴映射20个对象EtherCAT的通信负载会明显上升。我一般只映射实际用到的对象用不到的坚决不加。2.3 伺服参数设置那些容易忽略的细节IS620N的驱动器参数里有几个和轴控功能块密切相关的必须提前设好。H0D-00控制模式要设为EtherCAT总线模式否则驱动器不认总线指令。H0A-00绝对式编码器如果你的电机是绝对值编码器要设为1这样回零之后断电再上电不需要重新回零。H05-00位置指令来源要设为EtherCAT不然驱动器会走本地脉冲指令。还有一个容易被忽略的参数是H0B-00软件限位使能。如果你在功能块里做了软限位逻辑驱动器这边的软限位可以关掉避免双重限位导致动作异常。但如果你没在功能块里做软限位那驱动器这边的软限位一定要打开这是最后一道保护。另外H0C-00跟随误差窗口这个参数决定了伺服在定位过程中允许的跟随误差。设得太小稍微有点负载波动就报警设得太大定位精度又没保证。我的经验值是先按默认值跑观察实际运行中的跟随误差曲线再根据曲线来调整。3. 轴控功能块的接口设计3.1 输入输出变量怎么定功能块的接口设计是整个封装的核心。我最终定下来的接口是这样的输入变量分两组。第一组是配置参数在功能块初始化时设定运行中一般不改nAxisID轴号对应EtherCAT从站地址rSoftLimitPos正向软限位rSoftLimitNeg负向软限位rMaxVelocity最大速度限制rAcceleration加速度rDeceleration减速度rHomingVelocity回零速度第二组是控制指令每个扫描周期根据逻辑触发bEnable使能bReset故障复位bHome回零触发bJogPos正向点动bJogNeg负向点动bMoveAbsolute绝对定位触发rTargetPosition目标位置bMoveRelative相对定位触发rRelativeDistance相对距离bStop停止输出变量主要是状态反馈bReady就绪bBusy运行中bDone动作完成bError故障nErrorCode故障码rActualPosition实际位置rActualVelocity实际速度bHomed已回零这套接口看起来变量不少但实际调用的时候配置参数只在初始化时设一次运行时主要操作的就是那几个控制指令和状态反馈。3.2 状态机的设计思路功能块内部用一个状态机来管理整个轴的控制流程。状态定义如下状态值状态名说明0IDLE初始状态未使能10ENABLING使能中20READY已使能就绪30HOMING回零中40JOGGING点动中50MOVING定位中60STOPPING停止中70ERROR故障状态状态之间的转换条件很关键。比如从IDLE到ENABLING条件是bEnable为TRUE且无故障。从ENABLING到READY条件是伺服状态字的“Operation Enabled”位为1。从READY到MOVING条件是收到定位指令且目标位置在软限位范围内。这里有个容易出错的地方从MOVING回到READY的条件。不能简单地用“定位完成”来判断因为伺服在定位完成后可能还有一点整定时间。我的做法是同时判断“目标到达”位和“速度为零”位两个条件都满足才认为定位真正完成。3.3 为什么用ST语言而不是梯形图AM402支持梯形图、功能块图、ST等多种编程语言。轴控功能块我选择用ST语言来写原因有三个。第一状态机用ST写更清晰。梯形图适合写逻辑联锁但写状态机就很别扭要用大量的置位复位指令可读性差。ST的CASE语句写状态机一目了然。第二数学运算用ST更方便。软限位判断、速度限制、位置换算这些涉及数值计算的部分ST写起来比梯形图简洁得多。第三代码复用性更好。ST写的功能块可以很方便地导出和导入在不同项目之间复用。梯形图虽然也能封装成FB但跨项目移植的时候经常出问题。当然如果你团队里的人都只熟悉梯形图那用梯形图封装也不是不行只是代码量会大一些维护起来累一点。4. 功能块内部逻辑实现4.1 使能与状态字解析CiA402协议的状态字0x6041是一个16位的字每一位都有特定含义。功能块需要解析这些位来判断伺服当前的状态。关键位如下Bit 0Ready to Switch OnBit 1Switched OnBit 2Operation EnabledBit 3FaultBit 4Voltage EnabledBit 5Quick StopBit 6Switch On DisabledBit 10Target ReachedBit 12Setpoint Acknowledge使能的过程就是按照CiA402的状态机依次发送控制字。控制字0x6040的关键位Bit 0Switch OnBit 1Enable VoltageBit 2Quick StopBit 3Enable OperationBit 7Fault Reset使能的典型序列是先发0x0006Shutdown等状态字Bit 1为1再发0x0007Switch On等状态字Bit 2为1最后发0x000FEnable Operation此时伺服进入Operation Enabled状态。在ST代码里我用一个CASE语句来实现这个序列CASE nState OF 0: // IDLE wControlWord : 16#0006; IF (wStatusWord AND 16#0001) 0 THEN nState : 10; END_IF 10: // ENABLING wControlWord : 16#0007; IF (wStatusWord AND 16#0004) 0 THEN wControlWord : 16#000F; nState : 20; END_IF 20: // READY wControlWord : 16#000F; bReady : TRUE; // 处理各种控制指令 ... END_CASE这段代码看起来简单但实际调试的时候要注意状态字的位判断要用AND运算不能直接比较数值。因为状态字的其他位可能随时变化直接比较数值会漏掉很多情况。4.2 回零逻辑的实现回零是轴控里比较复杂的部分。IS620N支持多种回零模式我一般用CiA402的Homing Mode模式6通过设置Homing Method0x6098来指定回零方式。常用的回零方式有Method 1负限位回零Method 2正限位回零Method 17负限位Z信号回零Method 18正限位Z信号回零Method 33Z信号回零Method 35当前位置回零对于大多数应用Method 17或18是最可靠的因为限位开关提供粗定位Z信号提供精定位。但如果你的机械结构没有限位开关那就只能用Method 33或35。回零的流程是设置运行模式为6Homing设置Homing Method设置回零速度然后触发控制字的Bit 4Homing Operation Start。功能块需要监控状态字的Bit 10Target Reached和Bit 12Setpoint Acknowledge来判断回零是否完成。注意回零完成后一定要把当前位置设为0或者预设值。IS620N的H0A-01参数可以设置回零后的偏移量但我在功能块里也会做一次位置清零双重保险。4.3 定位与点动逻辑定位逻辑相对简单设置运行模式为8Cyclic Synchronous Position写入目标位置然后触发控制字的Bit 4。功能块监控状态字的Bit 10来判断是否到达。但这里有个细节目标位置要经过软限位检查。如果目标位置超出软限位范围功能块应该拒绝执行并报错而不是直接发给伺服。我见过有的项目没做这个检查结果伺服直接撞到机械限位把联轴器都撞坏了。点动逻辑稍微不同点动是速度控制模式模式3设置目标速度然后触发。点动的时候要实时监控软限位一旦到达限位就立即停止。// 软限位检查 IF rTargetPosition rSoftLimitPos THEN bError : TRUE; nErrorCode : 100; // 超出正向软限位 RETURN; END_IF IF rTargetPosition rSoftLimitNeg THEN bError : TRUE; nErrorCode : 101; // 超出负向软限位 RETURN; END_IF4.4 故障处理与复位故障处理是功能块里最容易被忽视但最重要的部分。IS620N的故障码通过0x603F对象读取常见的故障码有故障码含义处理方式0x2310过流检查电机接线和负载0x3210过压检查制动电阻和电源0x3220欠压检查电源电压0x7305编码器故障检查编码器线缆0x8611跟随误差过大检查负载和增益参数0x8A00超速检查速度限制功能块检测到故障后应该立即停止所有动作进入ERROR状态并输出故障码。复位的时候先发控制字的Bit 7Fault Reset等状态字的Bit 3Fault清零后再重新走使能序列。这里有个坑故障复位后不能立即重新使能。伺服驱动器需要一点时间来完成内部复位如果立即发使能指令可能会再次报错。我的做法是复位后延时200ms再使能实测下来很稳。5. 完整工程搭建与调试5.1 工程结构规划在CODESYS里新建工程后我建议按以下结构组织DeviceAM402主站EtherCAT_MasterEtherCAT主站IS620N_Axis1到IS620N_Axis88个伺服从站PLC_PRG主程序调用8个轴控功能块FB_AxisControl轴控功能块GVL_AxisConfig全局变量存放各轴的配置参数GVL_AxisStatus全局变量存放各轴的状态反馈这种结构的好处是清晰。主程序里只负责调用功能块和传递参数具体的控制逻辑都在功能块内部互不干扰。5.2 主程序调用示例主程序里每个轴的调用方式如下// 轴1调用 fbAxis1( nAxisID : 1, rSoftLimitPos : 1000.0, rSoftLimitNeg : -1000.0, rMaxVelocity : 500.0, rAcceleration : 1000.0, rDeceleration : 1000.0, rHomingVelocity : 100.0, bEnable : bEnableAxis1, bReset : bResetAxis1, bHome : bHomeAxis1, bJogPos : bJogPosAxis1, bJogNeg : bJogNegAxis1, bMoveAbsolute : bMoveAbsAxis1, rTargetPosition : rTargetPosAxis1, bMoveRelative : bMoveRelAxis1, rRelativeDistance : rRelDistAxis1, bStop : bStopAxis1 ); // 读取状态 bReadyAxis1 : fbAxis1.bReady; bBusyAxis1 : fbAxis1.bBusy; bErrorAxis1 : fbAxis1.bError; nErrorCodeAxis1 : fbAxis1.nErrorCode; rActualPosAxis1 : fbAxis1.rActualPosition;8个轴就是8次调用代码量很少而且每个轴的逻辑完全独立。加轴的时候只需要复制一份调用改一下轴号和配置参数就行。5.3 调试步骤与实测记录调试的时候我一般按以下步骤来第一步通信测试。先不使能伺服只检查EtherCAT通信是否正常。在CODESYS的EtherCAT诊断界面看从站是否在线PDO数据是否在刷新。如果通信不正常先查网线、查ESI文件版本、查从站地址。第二步使能测试。给单个轴发使能指令观察状态字的变化。如果使能失败检查控制字序列是否正确检查驱动器的控制模式参数是否设为EtherCAT。第三步点动测试。使能成功后先做低速点动确认电机转向和位置反馈方向一致。如果方向反了改驱动器的H0A-03参数或者改功能块里的方向取反逻辑。第四步回零测试。点动正常后做回零观察回零过程是否平稳回零完成后位置是否清零。第五步定位测试。回零完成后做绝对定位和相对定位观察定位精度和重复精度。第六步多轴联动测试。单轴都正常后做多轴联动观察EtherCAT通信负载和轴间同步性。我在实测中记录了一组数据8个轴同时以500mm/s的速度运行EtherCAT通信周期1ms通信负载大约在40%左右PLC的CPU占用率约35%。这个负载是安全的但如果轴数增加到16个就需要考虑把通信周期放宽到2ms。5.4 常见问题速查问题现象可能原因排查方法伺服不使能控制字序列错误检查状态字Bit 2是否置位使能后立即报错驱动器参数不对检查H0D-00和H05-00回零找不到Z信号回零速度太快降低回零速度定位精度差增益参数不合适调整速度环和位置环增益多轴不同步EtherCAT通信周期太长缩短通信周期或减少PDO映射偶发通信中断网线屏蔽不好更换屏蔽网线检查接地故障复位无效故障未真正清除检查故障码排除硬件问题提示如果伺服报跟随误差过大0x8611不要急着调大误差窗口先检查机械负载是否卡顿、联轴器是否松动。很多时候是机械问题导致的调参数只是掩盖问题。6. 封装后的收益与扩展思路6.1 实际项目中的收益这套功能块封装完成后我在后续三个项目里都直接复用了。最直观的收益是程序开发时间从原来的两周缩短到三天。原来每个轴要单独写逻辑、单独调试现在只需要配置参数和调用功能块。调试的时候也方便哪个轴出问题就看哪个功能块的输出不用在几千行梯形图里翻找。另一个收益是维护成本大幅降低。有一次客户反映某个轴定位不准我检查了功能块的参数发现是软限位设错了。改一个参数问题解决。如果是以前那种每个轴单独写逻辑的方式我得先找到那个轴的代码段再逐行检查至少要多花半天时间。还有一个隐性收益是代码质量提升。功能块封装迫使我认真设计状态机和异常处理而不是像以前那样“能跑就行”。结果就是程序更稳定现场故障率明显下降。6.2 可以继续扩展的方向这套功能块目前只支持基本的使能、回零、点动、定位功能。如果你的应用更复杂可以继续扩展凸轮同步增加电子凸轮功能用CODESYS的CamBuilder生成凸轮曲线功能块里增加凸轮耦合和解耦的逻辑。飞拍与位置比较增加Touch Probe功能映射0x60B8和0x60B9对象实现位置锁存和高速输出。力矩控制增加力矩模式映射0x6071和0x6077对象用于需要恒力矩控制的场合。多轴插补如果PLC支持可以用CODESYS的SoftMotion库实现多轴直线插补和圆弧插补。参数自整定在功能块里集成增益自整定逻辑根据负载惯量自动调整速度环和位置环增益。6.3 最后分享几个实操心得第一个心得功能块的版本管理很重要。我在功能块里加了一个版本号变量每次修改都记录版本和修改内容。这样在现场调试的时候一眼就能看出用的是哪个版本避免版本混乱。第二个心得配置参数和运行参数分开存储。配置参数存在GVL里掉电保持运行参数存在功能块内部每次上电重新初始化。这样即使功能块内部状态乱了重新上电就能恢复。第三个心得调试的时候善用Trace功能。CODESYS的Trace可以记录变量随时间的变化曲线调伺服的时候把状态字、控制字、实际位置、实际速度都加到Trace里一眼就能看出问题出在哪个环节。第四个心得不要过度封装。功能块不是越通用越好。如果为了兼容各种奇怪的应用场景把功能块做得无比复杂那还不如不封装。我的原则是覆盖80%的常用场景剩下20%的特殊需求单独处理。这套轴控功能块从最初的想法到最终稳定运行前后改了大概五六个版本。每次改都是在实际项目中遇到了新问题然后回来优化。如果你也在做类似的事情我的建议是先把基本功能跑通然后在实际项目中逐步完善不要一开始就追求完美。