汇川AM402 PLC轴控功能块封装实战：EtherCAT与IS620N伺服控制

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 功能块的引脚定义

我设计的这个轴控功能块，接口分三部分：输入引脚、输出引脚、输入输出引脚。输入引脚是外部给功能块的命令和参数，输出引脚是功能块反馈给外部的状态和错误信息，输入输出引脚主要是轴对象引用。

先看输入引脚的设计：

输出引脚：

输入输出引脚就一个：io_Axis，类型是AXIS_REF，这是CODESYS标准轴对象引用，每个轴实例化的时候把对应的轴对象传进来。

2.3 状态机的设计

整个功能块的核心是一个状态机。我把轴控的状态分成这么几个：

• IDLE（0）：空闲状态，轴未使能
• ENABLING（10）：正在使能
• READY（20）：已使能，等待命令
• HOMING（30）：正在回零
• MOVING（40）：正在定位
• STOPPING（50）：正在停止
• ERROR（90）：错误状态

状态之间的切换逻辑是这样的：上电默认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_CASE

3.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_IF

3.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_IF

3.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_IF

4. 完整实操过程与工程搭建

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或2ms，IS620N的最大速度响应频率是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 常见问题速查表

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次的状态切换存到一个数组里。出问题的时候把数组读出来，就能看到状态是怎么跳的，比单步调试快多了。这个功能在排查偶发性故障的时候特别有用。

这个功能块后续还可以继续扩展，比如加一个位置触发输出功能，到某个位置自动输出一个信号；或者加一个位置比较功能，实时比较多个轴的位置。运动控制这个东西，需求是无穷的，功能块也要跟着不断迭代。