TwinCAT3 - 回零

前言
1，多圈绝对值编码器
2，增量式编码器
3，单圈绝对值编码器
4，问题
5，场景

前言

回零算是运动最复杂的问题之一，官方文档也写得很乱，命名也很乱。因为TwinCAT版本更迭，NC的参数也很乱，所以想把回零这个事稍微捋清楚点，完全不敢奢求彻底讲清楚。

回零顾名思义就是回到零点，但实际上回零操作并不是回到了零点，而是找到参考点并停在一个不确定的位置。因此我认为叫寻参更为合适。TwinCAT中的回零主要涉及PositionBias和MC_Home怎么用，NC的参数怎么配置。

PositionBias和MC_Home的用法和编码器类型强相关。下面都是电子编码器，不讨论机械式编码器，编码器类型分为

多圈绝对值编码器：我最常用
增量式编码器：我偶尔用
单圈绝对值编码器：我没用过

写本文档时用的驱动器是SV605N，下面分别讲每种编码器的回零操作。

1，多圈绝对值编码器

多圈绝对值编码器自己能记住位置，因此只需要PositionBias，一般不需要MC_Home，找 Bias 的标准做法如下：

步骤1：上电使能，手动jog把轴运动到机械原点位置。
步骤2：读取轴的实际位置。假设 NC 里显示 Actual Position = 27.583 mm。
步骤3：将 -27.583 填到 NC 的 Encoder 参数里的 Position Bias。（注意负号）
步骤4：重启激活配置，观察到 Actual Position 变成 0.000 mm，完成。

那多圈绝对值编码器什么情况下会用到MC_Home？

软件临时设置位置，将 MC_Home.HomingMode := MC_Direct，然后调用MC_Home，直接设置当前位置，轴不动，类似于MC_SetPosition，重新上电就会丢失临时位置。常见于机加工设备的换夹具 / 换治具 / 可移基准。
编码器电池耗尽，退化成单圈绝对值编码器，需要用MC_Home回零。

2，增量式编码器

增量式编码器是MC_Home的主战场，MC_Home功能块长这样

Execute：上升沿触发功能块。
Position：发生位置锁存时设置的位置。如果没有机械原点，直接填0。有机械原点的话，还要多做一步，后面会讲到。
HomingMode：填MC_DefaultHoming就行
BufferMode：无效
Options：无效
bCalibrationCam：触发位置锁存的信号，从参考点开关的IO输入取就行了。

分为3种回零模式。

Plc CAM，开关信号+软件锁存，默认的模式。
Hardware Latch，开关信号+硬件锁存。
Hardware Sync，开关信号+Z相脉冲+硬件锁存。Z相脉冲为编码器内置硬件信号，内部直连驱动器Probe单元。

3种模式的开关信号都是一样的，区别在于接线，模式选择，锁存位置的来源。

3种模式的切换靠的是NC参数，重点是Reference Mode，先把参数列举出来，后面按这套参数来执行，每个参数都会应用并解释。
简单起见，参数以TwinCAT 4020.32为例，新版本参数名称和数量都发生了变化，但是逻辑上没怎么变。

Invert Direction for Calibration Cam Search：搜索参考开关时，是否反转搜索运动方向。默认FALSE=沿正向运动去找参考开关，TRUE=轴沿负向运动去找参考开关。
Invert Direction for Sync Impuls Search：找到参考开关后，是否反转运动方向。FALSE=找到参考开关后，沿正向运动。TRUE=找到参考开关后，沿负向运动。
Calibration Value：效果就是MC_Home的Position引脚。当MC_Home.Position = 0时，以Calibration Value为准，否则以MC_Home.Position为准。
Reference Mode：7 个选项，决定回零模式。
- Default和Plc CAM：回零模式为Plc CAM
- Hardware Sync：回零模式为Hardware Sync
- Hardware Latch 1 (pos. edge)和Hardware Latch 1 (neg. edge)：回零模式为Hardware Latch
- Software Sync：回零模式为软件同步，单圈绝对值编码器专用，暂时不管
- Application(PLC code)：回零模式为PLC自定义，暂时不管

Homming Velocity (towards plc cam)：搜索参考开关的速度（一般为高速，搜索速度）
Homing Velocity (off plc cam)：离开参考开关的速度（一般为低速，精定位速度）

2.1 Plc CAM

Reference Mode选Default或Plc CAM。就接1根线，从参考开关接到IO输入，比如EL1809。代码如下

Home: MC_Home;

Home(Execute := bStart,  			 // 启动信号
     Axis := Axis1,					 // NC轴
     bCalibrationCam := bIOInput,  	 // 参考开关信号，从IO输入来
     HomingMode := MC_DefaultHoming, // 默认
     Position := 0.0                 // 触发锁存时，当前位置设置为0
);

功能块动作流程如下：

轴向参考开关运动，沿正向运动去找参考开关。
碰到参考开关（bCalibrationCam上升沿），开始减速，减速到0后反向运动
离开参考开关（bCalibrationCam下降沿），NC记住下降沿时编码器的位置，这个位置就是位置锁存值
因为参考点的位置设为0时还在减速，所以轴停下来时，编码器位置可不是0哦。回零完成，NC根据位置锁存值建立坐标系，参考点位置的坐标 = MC_Home.Position，然后置位MC_Home.Done = TRUE，Axis.Status.Homed = TRUE

这种回零方式，就靠一个参考开关信号，受扫描周期影响，所以精度不高，可能在几十微米。

2.2 Hardware Latch

Reference Mode选Hardware Latch 1 (pos. edge)或Hardware Latch 1 (neg. edge)。代码和Plc CAM一模一样。
接2根线，第1根从参考开关接到IO输入，第2根从参考开关到驱动器Latch端子。

Hardware Latch 1 (pos. edge)的功能块动作流程如下：

轴向参考开关运动，沿正向运动去找参考开关，目标速度为Homming Velocity (towards plc cam)。
碰到参考开关（bCalibrationCam上升沿），此时驱动器硬件电路瞬间锁存编码器当前位置(存放在PDO=0x60BA)，并且置位PDO=0x60B9的Bit1，开始减速。
轴减速到0后反向运动，目标速度为Homming Velocity (off plc cam)。
离开参考开关（bCalibrationCam下降沿），开始减速。
轴停下来时，回零完成。NC读驱动器的PDO=0x60BA，保存在NC.Enc.Inputs.nInData3 (Touch probe pos1 pos value)。NC根据位置锁存值建立坐标系，参考点位置的坐标 = MC_Home.Position，然后置位MC_Home.Done = TRUE，Axis.Status.Homed = TRUE。
注意参考点位置是0，在参考点才开始减速的，所以轴停下来时当前位置不是0哦。

Hardware Latch 1 (neg. edge)的功能块动作流程如下：

轴向参考开关运动，沿正向运动去找参考开关，目标速度为Homming Velocity (towards plc cam)。
碰到参考开关（bCalibrationCam上升沿）。开始减速。
减速到0后反向运动，目标速度为Homming Velocity (off plc cam)。
离开参考开关（bCalibrationCam下降沿），此时驱动器硬件电路瞬间锁存编码器当前位置(存放在PDO=0x60BB)，并且置位PDO=0x60B9的Bit2，开始减速。
轴停下来时，回零完成。NC读驱动器的PDO=0x60BB，保存在NC.Enc.Inputs.nInData4 (Touch probe pos1 neg value)。NC根据位置锁存值建立坐标系，参考点位置的坐标 = MC_Home.Position，然后置位MC_Home.Done = TRUE，Axis.Status.Homed = TRUE。
注意参考点位置是0，在参考点才开始减速的，所以轴停下来时当前位置不是0哦。

这种回零方式，主要bCalibrationCam上升沿或下降沿的锁存位置，精度较高，可能在几百纳米。

2.3 Hardware Sync

Reference Mode选Hardware Sync。代码和Plc CAM一模一样。
接2根线，第1根从参考开关接到IO输入，第2根从参考开关到驱动器Latch端子。

轴向参考开关运动，沿正向运动去找参考开关，目标速度为Homming Velocity (towards plc cam)。
碰到参考开关（bCalibrationCam上升沿）。开始减速。
减速到0后反向运动，目标速度为Homming Velocity (off plc cam)。
离开参考开关（bCalibrationCam下降沿），驱动器硬件进入等待 Z 相脉冲状态。
Z 相脉冲到达，驱动器硬件瞬间锁存编码器当前位置(存放在PDO=0x60BA)，并且置位PDO=0x60B9的Bit1，开始减速。
轴停下来时，回零完成。NC读驱动器的PDO=0x60BA，保存在NC.Enc.Inputs.nInData3 (Touch probe pos1 pos value)。NC根据位置锁存值建立坐标系，参考点位置的坐标 = MC_Home.Position，然后置位MC_Home.Done = TRUE，Axis.Status.Homed = TRUE。

和Hardware Latch 1 (neg. edge)很像，有2个区别，区别1就是参考点从bCalibrationCam下降沿变成了bCalibrationCam下降沿之后的首个Z相脉冲。区别2是锁存位置保存在PDO=0x60BA，而不是PDO=0x60BB。顺便说明一下0x60B9，其Bit1=TP1 pos edge stored(上升沿锁存完成)，Bit2=TP1 neg edge stored(下降沿锁存完成)，NC是自动读的，不用PLC管。这种回零方式精度最高，可能在几十纳米。

3，单圈绝对值编码器

当作增量式编码器处理就差不多了。

4，问题

4.1 增量式编码器机械原点

聪明的你可能发现了，使用增量式编码器时只提到了参考点，还把参考点的位置设置成了0，压根就没提到机械原点，这对吗？？这要分两种情况讨论。

情况1：结构上没设计机械原点，此时就用参考点做机械原点，把参考点设0就完事了。
情况2：结构上设计了机械原点，并且机械原点和参考点是不可能完全重合的，又希望机械原点的位置是0，就还要做一些操作，有2种做法。但不论是哪种做法，每次重新上电都要执行回零操作。只不过不用再手动找机械原点了。
- 做法1
  - 步骤1：手动jog把轴运动到机械原点位置。
  - 步骤2：读取轴的实际位置。假设 NC 里显示 Actual Position = 27.583 mm。
  - 步骤3：将 -27.583 填到 NC 的 Encoder 参数里的 Position Bias。
  - 步骤4：重启激活配置，再次执行回零操作。
- 做法2：
  - 步骤1：手动jog把轴运动到机械原点位置。
  - 步骤2：读取轴的实际位置。假设 NC 里显示 Actual Position = 27.583 mm。
  - 步骤3：将 -27.583 填到 MC_Home.Position中，再次执行回零操作。

4.2 PDO

除开EtherCAT伺服必须的PDO(比如0x6040，0x6041等)。对于MC_Home，哪些PDO是必须的？

Plc CAM：无
Hardware Latch(pos.edge)和Hardware Sync：0x60B9(Touch Probe Status) + 0x60BA(Touch Probe Pos1)
Hardware Latch(neg.edge)：0x60BB(Touch Probe Pos1 Neg)

配好PDO之后，重新链接NC轴，如果观察到这些PDO变量链接到NC轴的变量，说明配置生效了。

4.3 接线

怎么理解接到Latch端子？？接线完全是我的盲区了，而且每种驱动器都不一样。

参考IS620N的说明书（因为手头没有SV605N的说明书），把开关信号接到 SV605N 的 CN1 的 DI8，汇川官方已经把它定义为 TouchProbe 了。不知道SV605N是不是也一样。

5，场景

理论归理论，但现实可不是这样的。不同的场景的逻辑就会不同。

设想这么一个场景，驱动器用的多圈绝对值编码器，没有设计机械原点，原点全靠参考开关，参考开关经常挪动，参考开关只接到PLC不接驱动器，需要一键回零的功能，不能影响设备运行，用户可能在离参考点非常远的位置回零，参考开关不动时要求每次上电位置都要对。该如何设计整个流程呢？

先分析一下要求：

没有设计机械原点，原点全靠参考开关，参考开关经常挪动：没法按照多圈绝对值编码器的玩法搞了，开关动了就要重新寻参。
参考开关只接到PLC不接驱动器：只能用Plc CAM模式，Hardware模式硬件连线路都没有。
用户可能在离参考点非常远的位置回零：MC_Home可能也不适用，因为其寻参方向，寻参速度加速度用户接触不到，改不了，需要用MC_MoveVelocity和MC_Stop来模拟整个MC_Home流程。
参考开关不动时要求每次上电位置都要对：要做断电保持。有2种思路，第1种是用MC_WriteParameterPersistent+PositionBias，把每次寻参的PositionBias保持住。第2种是自定义一个Persistent变量，就叫CurrentPosition吧，记录当前位置，自己做断电保持，每次重新上电后用MC_SetPosition把CurrentPosition写回到轴。
第1种利用了多圈绝对值编码器能记住位置的优点，参考开关不动就是常规的Plc CAM模式。第2种不论是多圈绝对值编码器还是增量式编码器都适用，但是断电后的位移就无法记录，没用上多圈绝对值编码器的优点。综合考虑选用第1种。
不能影响设备运行：MC_WriteParameterPersistent+PositionBias不是立刻生效的，需要激活配置才能使用，激活配置就影响设备运行了。因此本次寻参过程，还需要临时用MC_Home(MC_Direct)或MC_SetPosition把参考点的位置拉到0。

好，分析完了来写代码

PROGRAM MAIN
VAR
    //参数----------
    g_SearchVelocity   : LREAL := 10.0;  // 高速搜索速度 [mm/s]
    g_ApproachVelocity : LREAL := 1.0;   // 低速定位速度 [mm/s]
    g_Acceleration     : LREAL := 50.0;  // 加减速度 [mm/s²]
    g_SearchDirection  : BOOL := FALSE;  // 搜索方向 FALSE=正向, TRUE=负向

    //-----
    HomeState: INT := 0;		// 状态机当前步
    bHome: BOOL;          		// 回零指令
    bHomeTrigger: R_TRIG;		// 一键回零按钮上升沿
    AxisRef: AXIS_REF;			// NC轴
    fbMoveVel: MC_MoveVelocity;
    fbHalt: MC_Halt;
    fbWriteParamPersist: MC_WriteParameterPersistent;
    fbHome: MC_Home;			// 当作MC_SetPosition用
    fLockedPos: LREAL;			// 下降沿锁存的位置
    bCam AT%I*: BOOL;			// 参考开关，从IO输入来
    CamRiseTrigge: R_TRIG;		// 参考开关上升沿检测
    CamFallTrigge: F_TRIG;		// 参考开关下降沿检测
    bInitialOnCam: BOOL;		// 启动时是否已在开关上
    tTimeout: TON;      		// 超时定时器
    sErrorMsg: STRING;			// 错误消息
END_VAR
    
CASE HomeState OF
    0:
        // 空闲，等待一键回零按钮
        bHomeTrigger(CLK:= bHome);
        IF bHomeTrigger.Q THEN
            // 记录初始状态
            bInitialOnCam := bCam;
            sErrorMsg := '';
            HomeState := 10;
        END_IF
    10:
        // 如果初始就在开关上，先反向离开
        IF bInitialOnCam THEN
            fbMoveVel(Axis := AxisRef, Velocity := g_ApproachVelocity, 
                      Direction := SEL(g_SearchDirection, MC_Negative_Direction, MC_Positive_Direction), 
                      Acceleration := g_Acceleration, Execute := TRUE);
            // 等待离开开关（下降沿）
            CamFallTrigge(CLK:= bCam);
            IF CamFallTrigge.Q THEN
                fbMoveVel(Axis := AxisRef, Execute := FALSE);
                HomeState := 15; // 进入暂停等待
            END_IF
        ELSE
            HomeState := 20; // 直接开始搜索
        END_IF
    15: 
		// 短暂停顿确保离开
        fbHalt(Axis := AxisRef, Execute := TRUE);
        IF fbHalt.Done THEN
            fbHalt(Axis := AxisRef, Execute := FALSE);
            HomeState := 20;
        END_IF
    20:
		// 高速搜索参考开关
        fbMoveVel(Axis := AxisRef, Velocity := g_SearchVelocity, 
                  Direction := SEL(g_SearchDirection, MC_Positive_Direction, MC_Negative_Direction), 
                  Acceleration := g_Acceleration, Execute := TRUE);
		// 等待上升沿
		CamRiseTrigge(CLK:= bCam);
        IF CamRiseTrigge.Q THEN
            fbMoveVel(Axis := AxisRef, Execute := FALSE);
            HomeState := 30;
        END_IF
        // 超时保护（例如 30 秒）
        tTimeout(IN:=TRUE, PT:=T#30S);
        IF tTimeout.Q THEN
           	tTimeout(IN:=FALSE);
            fbMoveVel(Axis := AxisRef, Execute := FALSE);
            sErrorMsg := '搜索参考开关超时';
            HomeState := 98;
        END_IF
    30: 
		// 停止，准备反向低速定位
        fbHalt(Axis := AxisRef, Execute := TRUE);
        IF fbHalt.Done THEN
            fbHalt(Axis := AxisRef, Execute := FALSE);
            HomeState := 40;
        END_IF
    40:
		// 反向低速定位，等待下降沿
        fbMoveVel(Axis := AxisRef, Velocity := g_ApproachVelocity, 
                  Direction := SEL(g_SearchDirection, MC_Negative_Direction, MC_Positive_Direction), 
                  Acceleration := g_Acceleration, Execute := TRUE);
        // 等待下降沿
        CamFallTrigge(CLK:= bCam);
        IF CamFallTrigge.Q THEN
            fbMoveVel(Axis := AxisRef, Execute := FALSE);
            // 锁存当前位置，软件锁存，精度受扫描周期影响
            fLockedPos := AxisRef.NcToPlc.ActPos;
            HomeState := 50;
			// 停轴
            fbHalt(Axis := AxisRef, Execute := TRUE);
        END_IF
        // 超时保护
        tTimeout(IN:=TRUE, PT:=T#30S);
        IF tTimeout.Q THEN
            tTimeout(IN:=FALSE);
            fbMoveVel(Axis := AxisRef, Execute := FALSE);
            sErrorMsg := '定位参考开关超时';
            HomeState := 98;
        END_IF
    50:
		// 等待轴停止
        fbHalt(Axis := AxisRef, Execute := TRUE);
        IF fbHalt.Done THEN
            fbHalt(Axis := AxisRef, Execute := FALSE);
            HomeState := 60;
        END_IF
    60: 
		// 计算并写入 PositionBias，下次上电 PositionBias 才会生效
        fbWriteParamPersist(ParameterNumber := MC_AxisParameter.AxisEncoderOffset, // 对应 Position Bias
                            Value := -fLockedPos,  // 取反
                            Axis := AxisRef,
                            Execute := TRUE);
        IF fbWriteParamPersist.Done THEN
            fbWriteParamPersist(Axis := AxisRef, Execute := FALSE);
            HomeState := 70;
        ELSIF fbWriteParamPersist.Error THEN
            sErrorMsg := '写入Bias失败';
            HomeState := 98;
        END_IF
    70:
        // 修改当前位置为 ActPos - fLockedPos，修改后参考点（锁存点）坐标 = 0
        fbHome(Axis := AxisRef, Execute := TRUE, HomingMode := MC_Direct, Position := AxisRef.NCToPlc.ActPos - fLockedPos);
        IF fbHome.Done THEN
            fbHome(Axis := AxisRef, Execute := FALSE);
            HomeState := 0;
            // 轴最终停在非0的位置，可自己加逻辑把轴走到0位置去
        ELSIF fbHome.Error THEN
            sErrorMsg := 'MC_Direct修正坐标失败';
            HomeState := 98;
        END_IF
    98: 
        // 错误处理，停轴
        fbHalt(Axis := AxisRef, Execute := TRUE);
        IF fbHalt.Done THEN
            fbHalt(Axis := AxisRef, Execute := FALSE);
            HomeState := 0;
        END_IF
END_CASE

上面的代码编译过了，但还没实际跑过，要谨慎调试。

posted @ 2026-07-13 15:27 tossorrow 阅读(32) 评论(0) 收藏举报

刷新页面返回顶部

tossorrow