1. 为什么我要自己封装轴控功能块汇川AM402这款PLC在中小型运动控制项目里出场率非常高搭配IS620N伺服驱动器走EtherCAT总线整套方案性价比很能打。但如果你用过原厂提供的库或者直接调MC_Power、MC_MoveAbsolute这些标准功能块就会发现一个问题每个轴都要重复写一大堆逻辑回零、使能、报警复位、限位处理、状态机切换项目里轴一多程序就变成复制粘贴的灾难现场。我手上这个项目一共6个轴3个定位轴加3个跟随轴如果每个轴都单独写一套控制逻辑程序量直接爆炸后期改一个参数要改6个地方调试的时候找问题能找疯。所以我的做法是把轴控的通用逻辑全部封装成一个功能块每个轴只需要实例化一次传入对应的轴对象和参数就行。这样程序结构清晰维护成本直线下降新项目直接把这个功能块复制过去就能用。这篇文章我会把整个封装过程拆开讲清楚包括为什么这么设计、每个引脚怎么定义、状态机怎么跑、回零逻辑怎么处理、报警怎么复位、限位怎么保护以及实际调试中踩过的坑。最后附上完整的工程结构说明你可以直接照着搭。注意本文基于汇川AM402 PLC配合IS620N伺服驱动器的EtherCAT总线控制场景使用InoProShop基于CODESYS编程环境。不同型号的PLC或驱动器在细节上可能有差异但整体思路是通用的。2. 整体设计思路与功能块架构拆解2.1 为什么要用功能块而不是函数在CODESYS体系里功能块FB和函数FC的区别很关键。函数没有内部状态每次调用都是独立的适合做纯计算功能块有内部状态可以保存上一次调用的数据适合做需要记忆的控制逻辑。轴控这个东西天然需要状态记忆——轴当前是在使能状态还是回零状态还是定位状态这些信息必须跨扫描周期保持所以必须用功能块。另一个原因是功能块支持实例化每个轴一个实例实例之间数据完全隔离。你用FB_AxisControl实例化6次得到6个独立的轴控对象互不干扰。如果用函数你得把所有状态变量都定义成全局变量程序一复杂就乱套了。2.2 功能块的引脚定义我设计的这个轴控功能块接口分三部分输入引脚、输出引脚、输入输出引脚。输入引脚是外部给功能块的命令和参数输出引脚是功能块反馈给外部的状态和错误信息输入输出引脚主要是轴对象引用。先看输入引脚的设计引脚名数据类型说明i_xEnableBOOL轴使能命令上升沿触发使能流程i_xHomeBOOL回零命令上升沿触发回零流程i_xMoveAbsBOOL绝对定位触发i_xMoveRelBOOL相对定位触发i_xStopBOOL停止命令i_xResetBOOL报警复位i_rTargetPosLREAL目标位置i_rTargetVelLREAL目标速度i_rAccLREAL加速度i_rDecLREAL减速度i_rHomeVelFastLREAL回零快速速度i_rHomeVelSlowLREAL回零慢速i_rHomeAccLREAL回零加速度i_rSoftLimitPosLREAL正向软限位i_rSoftLimitNegLREAL负向软限位输出引脚引脚名数据类型说明q_xEnabledBOOL轴已使能q_xHomedBOOL轴已回零q_xBusyBOOL轴正在执行动作q_xDoneBOOL动作完成q_xErrorBOOL轴有错误q_iErrorIDINT错误代码q_rActPosLREAL当前位置q_rActVelLREAL当前速度q_iStateINT当前状态机状态输入输出引脚就一个io_Axis类型是AXIS_REF这是CODESYS标准轴对象引用每个轴实例化的时候把对应的轴对象传进来。2.3 状态机的设计整个功能块的核心是一个状态机。我把轴控的状态分成这么几个IDLE0空闲状态轴未使能ENABLING10正在使能READY20已使能等待命令HOMING30正在回零MOVING40正在定位STOPPING50正在停止ERROR90错误状态状态之间的切换逻辑是这样的上电默认IDLE收到使能命令后进入ENABLING使能成功进入READY。在READY状态下收到回零命令进入HOMING回零完成回到READY。收到定位命令进入MOVING定位完成回到READY。任何状态下收到停止命令进入STOPPING停止完成回到READY。任何状态下检测到错误进入ERROR复位成功后回到IDLE重新开始。这个状态机的好处是逻辑清晰每个状态只做该做的事不会出现命令冲突。比如轴正在回零的时候你按定位按钮状态机在HOMING状态不响应定位命令避免了误动作。2.4 为什么不用MC_MoveAbsolute直接做CODESYS标准库里有现成的MC_Power、MC_Home、MC_MoveAbsolute、MC_MoveRelative、MC_Stop、MC_Reset这些功能块功能很全。但直接用的问题是每个轴要调6个功能块6个轴就是36个功能块实例程序里密密麻麻全是方块看着就头疼。而且这些功能块之间的协调逻辑要你自己写比如使能没完成不能回零回零没完成不能定位这些互锁逻辑每个轴都要写一遍。封装成一个大功能块之后外部只需要调一个块传几个参数内部自动处理所有互锁和状态切换。程序清爽了出错概率也低了。3. 核心细节解析与实操要点3.1 轴对象的传递与EtherCAT配置在AM402上走EtherCAT控制IS620N第一步是在InoProShop的设备树里添加EtherCAT主站然后扫描从站。IS620N的ESI文件需要提前装好不然扫描不到。扫描到之后把伺服轴添加到运动控制轴组里每个轴会自动生成一个AXIS_REF对象。这里有个细节轴的名称和编号要记清楚后面功能块实例化的时候要一一对应。我一般会在变量声明区给每个轴定义一个AXIS_REF变量命名规范是Axis_1、Axis_2这样然后在EtherCAT配置里把轴对象映射到对应的变量上。IS620N的PDO配置也很关键。默认的PDO映射可能不包含你需要的所有数据比如力矩、电流、报警码这些。我一般会手动调整PDO把状态字、控制字、位置实际值、速度实际值、力矩实际值、报警码都加进去。这样功能块里可以直接读到这些数据不用额外做通信。提示PDO配置改完之后一定要重新下载配置并重启PLC不然映射不生效。我在这上面浪费过两个小时一直以为程序问题其实是PDO没生效。3.2 使能逻辑的实现细节使能看起来简单调个MC_Power就行但实际有几个坑。第一个坑是使能条件。MC_Power的Enable引脚不能一直给TRUE要用上升沿触发。如果一直给TRUE轴报警复位后会自动重新使能这在某些场景下是危险的。我的做法是在功能块内部做一个上升沿检测外部给一个脉冲就行。第二个坑是使能超时。如果伺服驱动器有故障没复位MC_Power会一直处于Busy状态程序就卡在ENABLING状态出不来。我加了一个超时计时器使能命令发出后3秒还没完成就报错进入ERROR状态。第三个坑是使能和回零的顺序。有些伺服驱动器要求先使能才能回零有些可以独立回零。IS620N是必须先使能。所以状态机里HOMING状态的前置条件是READY状态而READY状态的前提是使能完成。使能部分的代码逻辑大概是这样CASE iState OF 0: // IDLE IF i_xEnable THEN iState : 10; tonEnable(IN : TRUE, PT : T#3S); END_IF 10: // ENABLING fbPower(Axis : io_Axis, Enable : TRUE, Enable_Positive : TRUE, Enable_Negative : TRUE); IF fbPower.Status THEN iState : 20; tonEnable(IN : FALSE); ELSIF tonEnable.Q THEN iState : 90; iErrorID : 1001; // 使能超时 END_IF 20: // READY fbPower(Axis : io_Axis, Enable : TRUE, Enable_Positive : TRUE, Enable_Negative : TRUE); // 等待回零或定位命令 END_CASE3.3 回零逻辑的多种模式处理回零是轴控里最复杂的部分因为不同设备的机械结构不一样回零方式也不同。IS620N支持多种回零模式通过对象字典6040h的bit4-6或者607Ch来设置。常见的有模式1找原点开关然后找Z相信号模式2找原点开关然后找第一个Z相信号模式3直接找Z相信号模式17找负限位然后找Z相信号模式18找正限位然后找Z相信号我的功能块里把回零模式做成一个输入参数外部根据机械结构选择。回零速度也分快速和慢速两段快速找开关慢速找Z相这样精度更高。回零过程中有几个注意事项第一回零前必须确认轴已使能否则MC_Home会报错。第二回零过程中如果碰到硬限位要立即停止并报错不能硬撞。第三回零完成后要把当前位置设置为原点值这个值通常是0但有些设备需要设置为其他值所以做成参数。回零的代码逻辑30: // HOMING fbHome( Axis : io_Axis, Execute : xHomeTrig, Position : 0, Mode : iHomeMode, Velocity : i_rHomeVelFast, Acceleration : i_rHomeAcc, Deceleration : i_rHomeAcc ); IF fbHome.Done THEN q_xHomed : TRUE; iState : 20; ELSIF fbHome.Error THEN iState : 90; iErrorID : 2001; END_IF3.4 定位与软限位保护定位逻辑相对简单调MC_MoveAbsolute或MC_MoveRelative就行。但软限位保护必须做不然程序出错的时候轴会直接撞机械限位轻则丢步重则损坏设备。我的做法是在功能块内部做两层保护第一层是命令下发前检查目标位置是否在软限位范围内超出范围直接拒绝命令并报错第二层是运动过程中实时监控当前位置一旦超出软限位立即触发停止。软限位的值通过输入引脚传入每个轴可以设置不同的值。这里有个细节软限位要考虑回零后的坐标系。如果回零后原点在中间那正负软限位要分别设置如果原点在一端那只需要设置一个方向的限位。定位部分的代码40: // MOVING IF i_xMoveAbs AND NOT q_xBusy THEN IF (i_rTargetPos i_rSoftLimitNeg) AND (i_rTargetPos i_rSoftLimitPos) THEN fbMoveAbs( Axis : io_Axis, Execute : TRUE, Position : i_rTargetPos, Velocity : i_rTargetVel, Acceleration : i_rAcc, Deceleration : i_rDec ); q_xBusy : TRUE; ELSE iState : 90; iErrorID : 3001; // 目标位置超出软限位 END_IF END_IF IF fbMoveAbs.Done THEN q_xDone : TRUE; q_xBusy : FALSE; iState : 20; ELSIF fbMoveAbs.Error THEN iState : 90; iErrorID : 3002; END_IF // 实时软限位监控 IF (q_rActPos i_rSoftLimitPos) OR (q_rActPos i_rSoftLimitNeg) THEN fbStop(Axis : io_Axis, Execute : TRUE, Deceleration : i_rDec); iState : 90; iErrorID : 3003; // 超出软限位 END_IF3.5 报警处理与复位机制伺服报警是不可避免的关键是怎么处理。IS620N的报警码可以通过PDO读上来常见的有过流、过压、欠压、过载、编码器故障等。功能块里要做的第一件事是检测报警第二件事是分类处理。有些报警是可以自动复位的比如瞬时过载有些必须断电重启比如编码器故障。我的做法是在功能块里做一个报警等级判断一级报警自动复位二级报警需要手动复位三级报警直接停机。复位逻辑要注意MC_Reset只能复位轴控层面的错误驱动器层面的报警需要通过写控制字或者调用驱动器的复位功能块来清除。我一般是在功能块里先调MC_Reset如果复位不成功再尝试驱动器复位。90: // ERROR IF i_xReset THEN fbReset(Axis : io_Axis, Execute : TRUE); IF fbReset.Done THEN iState : 0; q_xError : FALSE; iErrorID : 0; END_IF END_IF // 报警检测 IF io_Axis.bError THEN q_xError : TRUE; iErrorID : io_Axis.iErrorID; END_IF4. 完整实操过程与工程搭建4.1 工程创建与设备配置打开InoProShop新建工程选择AM402型号。然后在设备树里添加EtherCAT主站扫描从站。IS620N的ESI文件如果没有需要先从汇川官网下载安装。扫描到伺服后把轴添加到运动控制轴组配置PDO映射。PDO配置我一般选的是控制字6040h状态字6041h目标位置607Ah实际位置6064h目标速度60FFh实际速度606Ch实际力矩6077h报警码603Fh运行模式6060h这些PDO映射好之后轴对象里就能直接读到这些数据。4.2 功能块代码编写新建一个功能块命名为FB_AxisControl语言选ST结构化文本。按照前面说的引脚定义声明变量。然后写状态机代码。这里有个技巧把状态机的每个状态写成CASE分支每个分支里只写该状态的处理逻辑。状态切换用变量赋值不要用GOTO。这样代码可读性好调试的时候也能清楚地看到当前在哪个状态。功能块的主体代码大概300行左右包含使能、回零、定位、停止、复位、报警处理、软限位保护。代码写完之后编译确保没有语法错误。4.3 主程序调用与轴实例化在主程序里声明6个功能块实例fbAxis1: FB_AxisControl; fbAxis2: FB_AxisControl; fbAxis3: FB_AxisControl; fbAxis4: FB_AxisControl; fbAxis5: FB_AxisControl; fbAxis6: FB_AxisControl;然后在程序里依次调用把对应的轴对象和参数传进去fbAxis1( io_Axis : Axis_1, i_xEnable : xEnableAxis1, i_xHome : xHomeAxis1, i_xMoveAbs : xMoveAxis1, i_rTargetPos : rTargetPos1, i_rTargetVel : 100.0, i_rAcc : 1000.0, i_rDec : 1000.0, i_rSoftLimitPos : 500.0, i_rSoftLimitNeg : -500.0 );每个轴都这样调用参数根据实际机械调整。这样主程序里只有6个功能块调用非常清爽。4.4 参数计算与设置速度、加速度这些参数不能随便填要根据机械结构算。比如丝杆导程10mm电机编码器分辨率23位8388608脉冲/转那一个脉冲对应的直线距离是10mm / 8388608 ≈ 0.00000119mm。如果要求线速度0.8米/秒那电机转速是0.8 * 60 / 0.01 4800rpm对应的脉冲频率是4800 * 8388608 / 60 ≈ 671MHz这个频率太高了实际EtherCAT周期可能达不到。所以实际项目中速度参数要根据EtherCAT周期和驱动器能力来设置。AM402的EtherCAT周期一般是1ms或2msIS620N的最大速度响应频率是3.2kHz左右。我一般会把最大速度限制在驱动器额定转速的80%以内留余量。加速度的设置要考虑机械惯量和电机力矩。惯量比大的时候加速度不能设太高不然会过冲或者报警。我一般从1000mm/s²开始试慢慢往上加直到满足节拍要求又不报警。4.5 调试步骤与验证方法调试分几步走第一步单独测试每个轴的使能和回零。手动触发使能看轴是否上电锁定手动触发回零看轴是否正常找原点。第二步测试定位功能。给一个小的相对定位命令看轴是否正常移动位置是否准确。第三步测试软限位。故意给一个超出软限位的目标位置看功能块是否拒绝并报错。第四步测试报警复位。手动触发伺服报警比如断开编码器线看功能块是否检测到并报错复位后是否恢复正常。第五步多轴联动测试。同时给多个轴发命令看是否互相干扰。注意调试的时候一定要把急停按钮准备好程序出错的时候能立即切断使能。我见过有人调试的时候轴飞车把机械撞坏的。5. 常见问题与排查技巧实录5.1 使能失败排查使能失败是最常见的问题。排查顺序是先看伺服驱动器面板有没有报警有报警先清报警再看EtherCAT通信是否正常从站是否在线然后看PDO映射是否正确控制字和状态字是否在映射里最后看MC_Power的ErrorID根据错误码查手册。常见错误码SMC_ErrorCode里的SMC_ERR_AXIS_DISABLED表示轴未使能SMC_ERR_AXIS_NOT_READY表示轴未就绪。这些一般是驱动器侧的问题检查驱动器参数和接线。5.2 回零异常排查回零异常一般有几种表现回零找不到原点、回零后位置不对、回零过程中报错。找不到原点通常是原点开关信号没接到PLC或者接反了。检查DI输入点看信号是否正常。回零后位置不对一般是Z相信号问题IS620N的Z相可以通过参数设置是否使用。回零过程中报错可能是速度太快撞到限位降低回零速度试试。5.3 定位精度问题排查定位精度不够先看机械有没有间隙联轴器有没有松动。然后看伺服增益是否调好增益太低响应慢增益太高会振荡。再看EtherCAT周期是否稳定周期抖动大会影响定位精度。我遇到过一次定位偏差0.1mm的问题查了半天发现是丝杆热膨胀导致的后来加了光栅尺做全闭环才解决。所以精度要求高的场合半闭环是不够的。5.4 常见问题速查表问题现象可能原因排查方法解决方案使能失败驱动器报警查看驱动器面板清除报警后重试使能失败EtherCAT断线查看从站状态检查网线连接回零找不到原点原点开关信号异常监控DI输入检查接线和开关回零后位置不对Z相信号未使用查看驱动器参数启用Z相或调整回零模式定位偏差大机械间隙手动检查机械调整机械或加补偿定位偏差大伺服增益不足查看跟随误差调整增益参数运动过程中报警加速度太大查看报警码降低加速度软限位误触发软限位设置错误检查参数重新设置软限位值5.5 独家避坑技巧第一个坑EtherCAT从站掉线后重新上线轴对象的状态可能没有正确复位。我的做法是在功能块里加一个通信状态检测一旦检测到从站掉线立即把状态机复位到IDLE等通信恢复后重新使能。第二个坑多个轴同时回零的时候如果共用一个原点开关可能会互相干扰。解决方法是分时回零或者每个轴用独立的原点开关。第三个坑功能块实例化的时候如果忘记把轴对象传进去编译不会报错但运行的时候轴不动。这个错误很隐蔽我建议在功能块内部加一个轴对象有效性检查无效的时候直接报错。第四个坑IS620N的制动电阻选型。如果负载惯量大或者频繁启停制动电阻功率不够会导致母线过压报警。选型的时候要算制动能量宁大勿小。第五个坑程序下载后第一次运行轴可能会轻微抖动一下。这是使能瞬间的电流冲击一般不影响使用但如果抖动太大要检查伺服增益和使能顺序。6. 功能块扩展与项目复用6.1 增加力矩限制功能标准功能块里没有力矩限制但实际项目中经常需要。比如夹取动作力矩到了就停不能把工件夹坏。扩展方法是增加一个输入引脚i_rTorqueLimit在MC_MoveAbsolute之前设置驱动器的力矩限制参数或者用MC_TorqueLimit功能块。IS620N的力矩限制可以通过对象字典3521h设置也可以通过PDO写。我一般是在功能块里加一个判断如果力矩限制值大于0就启用限制否则用驱动器默认值。6.2 增加多段速定位功能有些设备需要多段速定位比如先快速走到接近位置再慢速走到目标位置。扩展方法是增加一个位置数组和速度数组功能块内部按顺序执行。这个功能在贴片机、点胶机上很常用。实现思路是状态机增加一个MOVING_MULTI状态内部维护一个步骤计数器每完成一段就切换到下一段直到所有段完成。6.3 增加轴间同步功能多轴同步是运动控制的高级功能比如龙门结构的双轴同步、飞剪的追剪同步。CODESYS里有MC_GearIn、MC_CamIn这些功能块可以做同步。我的做法是在轴控功能块基础上再封装一个同步功能块把主轴和从轴的对象传进去内部调MC_GearIn。同步功能块的难点是同步解除和重新同步的逻辑。我一般会在同步状态里加一个监控如果同步误差超过阈值立即解除同步并报错防止机械损坏。6.4 工程复用注意事项这个功能块封装好之后新项目可以直接复制。但要注意几点第一轴对象的名称要改每个项目的轴命名可能不一样第二PDO映射要重新配置不同驱动器的PDO可能不同第三参数要重新计算机械结构不同参数肯定不同第四回零模式要确认不同设备的回零方式可能不一样。我一般会把这个功能块做成一个库文件新项目直接添加库然后实例化调用。库文件里把参数默认值设好新项目只需要改几个关键参数就行。提示库文件做好之后要版本管理每次修改都记录改了什么不然时间长了自己也忘了。7. 实际调试中的个人体会这个轴控功能块我从第一版到现在用了三年多中间改过很多次。最开始的时候没有状态机就是一堆IF-ELSE堆在一起程序跑起来经常出现命令冲突轴一会儿动一会儿停。后来加了状态机逻辑清晰多了但状态切换的条件没写好出现过卡在某个状态出不来的情况。再后来加了超时检测和错误处理稳定性才上来。我个人觉得封装功能块最大的价值不是省代码而是把控制逻辑标准化。每个轴都用同一套逻辑调试的时候只需要关注参数不需要关注逻辑。新来的同事接手项目看一遍功能块就知道轴是怎么控制的学习成本很低。另外功能块的注释一定要写清楚。我见过有人功能块写得很漂亮但一个注释都没有过两个月自己都看不懂了。我的习惯是每个引脚都写注释每个状态都写说明关键逻辑写清楚为什么这么做。最后分享一个小技巧功能块调试的时候可以在状态机里加一个状态历史记录把最近10次的状态切换存到一个数组里。出问题的时候把数组读出来就能看到状态是怎么跳的比单步调试快多了。这个功能在排查偶发性故障的时候特别有用。这个功能块后续还可以继续扩展比如加一个位置触发输出功能到某个位置自动输出一个信号或者加一个位置比较功能实时比较多个轴的位置。运动控制这个东西需求是无穷的功能块也要跟着不断迭代。