同步轴 / 电子齿轮

基于 Darra EtherCAT Master 类库的 STF-EC 步进驱动器同步轴运动控制案例 (WinForms 界面)。

一句话说明：多个轴按各自的齿比 (Ratio)，跟随同一个虚拟主轴运动。

• 齿比 1:1：所有轴「同一数据点同时映射」，运动完全一致。
• 齿比 ≠ 1：即电子齿轮，各轴按固定比例联动。
• 运行模式恒为 CSP (周期同步位置, Mode=8)。

完整源码

本案例完整源码 (WinForms 工程 + config.deni)： https://github.com/DarraTechnology/Ethercat_Master/tree/main/Windows/CSharp/STF-EC_SyncAxis

硬件配置

• 主站: Windows 10 × 1 + Intel i5 × 1
• 步进驱动器: 鸣志 (Shanghai AMP&MOONS' Automation) STF-EC EtherCAT 步进驱动器 × 5 + 步进电机 × 5

项	值
厂商	鸣志 Shanghai AMP&MOONS' Automation
VendorId	0x00000168
ProductCode	0x02
协议	CoE (CiA 402 Profile)
同步方式	DC Sync0 = 125µs
轴数	由 config.deni 扫描决定 (提供的配置含 5 轴, PhysAddr 0x1001~0x1005)

与 STF-EC_CSP_PP 案例同款硬件、同一份 config.deni，可直接复用。

运动参数

• 控制模式: CSP (周期同步位置, Mode=8) — 同步联动只用 CSP
• 每转脉冲数: PULSES_PER_REV = 10000 (STF-EC 默认细分，位置/角度换算用)
• 虚拟主轴速度: 由用户设定 (脉冲/秒)，每周期推进步长 = 速度 ÷ 8000
• 齿比范围: 任意实数 (1.0 完全同步 / 2.0 主轴 2 倍 / 0.5 一半 / -1.0 反向等速)

性能指标

• 控制周期: 125µs (LoopCycle = DC Sync0)
• 多轴同步: 所有从轴严格锁定同一虚拟主轴相位，齿比 1:1 时各轴位置增量完全一致
• 位置精度: ±1 脉冲
• 启动平滑: 启动同步瞬间快照各轴实际位置作基准，零跳变

应用场景

电子齿轮 / 同步轴，用在需要多轴严格比例联动的工业场合。常见有：

• 收放卷同步: 放卷轴与收卷轴按线速比例同步，张力恒定。
• 龙门双驱同步: 龙门两侧驱动轴必须以 1:1 严格同步，防止机架扭斜卡死。
• 分切机: 主辊与多组分切刀辊按齿比联动。
• 印刷套色: 多个色组辊筒按固定比例同步走纸，保证套印精度。

把每个轴齿比设成不同值，肉眼即可看到比例联动效果。

工作原理

虚拟主轴

本例不需要真实的「主轴」硬件。它用一个软件计数器当虚拟主轴。各轴都跟随这个计数器运动：

• 自动推进: 点「启动同步」后，每个 PDO 周期 (约 125µs) 主轴位置 += 速度 ÷ 8000。例如速度 = 80000 脉冲/秒，则每周期 +10 脉冲。
• 手动点动: 按住「◀ 主轴-」/「主轴+ ▶」可直接推/退主轴位置，方便手动对位、观察比例运动。
• 启动瞬间快照: 点「启动同步」时，PDO 线程会重新快照各轴当前实际位置，作为基准 Base，同时把主轴清零。这样运动从「当前位置」平滑开始，不会跳变。

各轴按齿比跟随

每个真实轴的目标位置，都由同一个虚拟主轴位置 masterPos 算得。区别只在各轴的齿比 Ratio 不同：

目标位置 = Base + round(masterPos × Ratio)

齿比含义：

齿比 Ratio	含义
1.000	各轴目标增量完全相同 → 「同一数据点同时映射」，多轴齐步走
2.000	该轴走主轴的 2 倍 (主轴转 1 圈，从轴转 2 圈)
0.500	该轴走主轴的一半
-1.000	该轴反方向等速跟随

电子齿轮 = 电子凸轮的线性特例：电子凸轮用一张主从位置曲线让从轴跟随主轴；当曲线退化成一条直线 (固定斜率 = 齿比) 时，就是电子齿轮。

系统架构

代码示例

PDO 结构体定义

PDO 字节布局来自 config.deni 实测。字段顺序必须与 PDO 条目逐字节一致，不可调换。

using System.Runtime.InteropServices;

/// <summary>
/// STF-EC 步进驱动器输出 PDO (RxPDO) = 29 字节 (0x1600~0x1603)
/// 同步轴恒用 CSP, ModesOfOperation 永远 = 8。
/// </summary>
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct STF_Output  // 29 字节
{
    // ---- 0x1600 ----
    public ushort ControlWord;          // 0x6040 控制字 (CiA402 状态机)
    public sbyte ModesOfOperation;      // 0x6060 操作模式 (同步轴恒 CSP=8)
    public int TargetPosition;          // 0x607A 目标位置 (脉冲) ← 每周期下发
    // ---- 0x1601 ----
    public uint ProfileVelocity;        // 0x6081 轮廓速度 (本例不使用)
    public uint ProfileAcceleration;    // 0x6083 轮廓加速度
    public uint ProfileDeceleration;    // 0x6084 轮廓减速度
    // ---- 0x1602 ----
    public int TargetVelocity;          // 0x60FF 目标速度 (PV 模式用, 本例不使用)
    // ---- 0x1603 ----
    public uint PhysicalOutputs;        // 0x60FE:1 数字输出
    public ushort TouchProbeFunction;   // 0x60B8 探针功能
}

/// <summary>
/// STF-EC 步进驱动器输入 PDO (TxPDO) = 35 字节 (0x1A00~0x1A03)
/// 注意: 此 PDO 错误码在前、状态字在后。
/// </summary>
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct STF_Input  // 35 字节
{
    // ---- 0x1A00 ----
    public ushort ErrorCode;                // 0x603F 错误码
    public ushort StatusWord;               // 0x6041 状态字 (CiA402 状态机)
    public sbyte ModesOfOperationDisplay;   // 0x6061 操作模式显示
    // ---- 0x1A01 ----
    public int PositionActualValue;         // 0x6064 实际位置 (脉冲) ← 跟随误差 / Base 用
    // ---- 0x1A02 ----
    public int VelocityActualValue;         // 0x606C 实际速度 (脉冲/s)
    // ---- 0x1A03 ----
    public uint DigitalInputs;              // 0x60FD 数字输入
    public ushort TouchProbeStatus;         // 0x60B9 探针状态
    public int TouchProbe1PosValue;         // 0x60BA 探针1上升沿位置
    public int TouchProbe1NegValue;         // 0x60BB 探针1下降沿位置
    public int TouchProbe2PosValue;         // 0x60BC 探针2上升沿位置
    public int TouchProbe2NegValue;         // 0x60BD 探针2下降沿位置
}

连接与 DC 同步

CSP 同步联动要求每个 PDO 周期都精确下发新目标位置。所以连接阶段必须先做好三件事：

• 把 PDO 完整重映。
• 启用 DC (分布式时钟)。
• 把目标位置初始化为实际位置 (防上电跳变)。

以下为关键步骤 (省略了 try/catch、CancellationToken 取消检查等样板)。

// 1) 建主站: 载入 config.deni, 开启自动启动流程
var buildResult = new DarraEtherCAT()
    .SetENI(deniPath)
    .EnableAutoStartup()
    .Build();
DarraEtherCAT master = buildResult.Master;
int slaveCount = master.SlaveCount;          // 轴数 = 实际扫描到的从站数

// 2) 进 PreOp: 此时 SM2 未激活, 可经 CoE 改 PDO 映射
master.SetState(EcState.PreOp);

for (int i = 0; i < slaveCount; i++)
{
    var slave = master.Slaves[i];

    // 3) 经 CoE 把 PDO 分配重映成 config.deni 的完整 4+4 (输出29/输入35)。
    //    驱动默认 RxPDO 只有 11 字节, 不重映就和结构体对不上, 任何模式都会映射失败。
    slave.CoE.SDOWrite(0x1C12, 0, new byte[] { 0 });                          // 先清空 RxPDO 分配
    slave.CoE.SDOWrite(0x1C12, 1, BitConverter.GetBytes((ushort)0x1600));     // 装入 4 个输出 PDO 对象
    slave.CoE.SDOWrite(0x1C12, 2, BitConverter.GetBytes((ushort)0x1601));
    slave.CoE.SDOWrite(0x1C12, 3, BitConverter.GetBytes((ushort)0x1602));
    slave.CoE.SDOWrite(0x1C12, 4, BitConverter.GetBytes((ushort)0x1603));
    slave.CoE.SDOWrite(0x1C12, 0, new byte[] { 4 });                          // 写入条目数 = 4

    slave.CoE.SDOWrite(0x1C13, 0, new byte[] { 0 });                          // 先清空 TxPDO 分配
    slave.CoE.SDOWrite(0x1C13, 1, BitConverter.GetBytes((ushort)0x1A00));     // 装入 4 个输入 PDO 对象
    slave.CoE.SDOWrite(0x1C13, 2, BitConverter.GetBytes((ushort)0x1A01));
    slave.CoE.SDOWrite(0x1C13, 3, BitConverter.GetBytes((ushort)0x1A02));
    slave.CoE.SDOWrite(0x1C13, 4, BitConverter.GetBytes((ushort)0x1A03));
    slave.CoE.SDOWrite(0x1C13, 0, new byte[] { 4 });                          // 写入条目数 = 4

    // 4) 启用分布式时钟 Sync0 = 125µs (CSP 周期同步的时间基准, 与 LoopCycle 一致)
    if (slave.HasDC) slave.ConfigureDC(125000);   // Sync0 = 125µs
}

// 设定 PDO 交换周期 = 125µs (Windows 推荐最小周期); SYNC0 与 LoopCycle 应保持一致
master.Config.LoopCycle = 125000;   // PDO 交换周期 = 125µs (Windows 推荐最小周期)

// 5) 进 SafeOp
master.SetState(EcState.SafeOp);

// 6) PDO 尺寸自检: 用驱动实测字节数核对结构体, 防映射错位
int expOut = Marshal.SizeOf<STF_Output>();   // 29
int expIn  = Marshal.SizeOf<STF_Input>();    // 35
for (int i = 0; i < slaveCount; i++)
{
    if (master.Slaves[i].OutputsByteCount != expOut ||
        master.Slaves[i].InputsByteCount  != expIn)
        throw new Exception($"轴{i + 1} PDO 尺寸不符, 映射与 config.deni 不一致");
}

// 7) 进 OP 前逐轴初始化: CSP 模式 + 目标位置 = 当前实际位置 (避免上电跳变)
for (int i = 0; i < slaveCount; i++)
{
    ref var input  = ref master.Slaves[i].PDO.InputsMapping<STF_Input>();
    ref var output = ref master.Slaves[i].PDO.OutputsMapping<STF_Output>();
    output.ModesOfOperation = 8;                          // CSP
    output.TargetPosition   = input.PositionActualValue;  // 目标 = 实际, 零跳变
}

// 8) 进 OP, 开始周期通讯
master.SetState(EcState.OP);

电子齿轮齿比计算

同步轴的全部数学只有一行。每个从轴的目标位置，由三部分相加 (相乘) 得到：

• 启动同步时的基准位置 Base。
• 虚拟主轴位置 masterPos。
• 该轴齿比 Ratio。

即：目标位置 = Base + masterPos × Ratio。

// 核心公式 (单轴, 单周期):
//   目标位置 = Base + round(masterPos × Ratio)
a.CurrentTarget = a.Base + (int)Math.Round(masterPos * a.Ratio);

• Base: 启动同步瞬间快照的该轴实际位置 — 让运动从「当前点」平滑开始，不跳变。
• masterPos: 虚拟主轴位置，所有轴共用同一个值 → 各轴严格锁定同一相位。
• Ratio: 该轴齿比。1.0 时所有轴目标增量一致 (同一数据点同时映射)；≠1.0 即电子齿轮，按比例拉开。

电子齿轮就是电子凸轮的线性特例：把凸轮的主从位置曲线退化成斜率 = Ratio 的直线，公式即由查曲线退化为一次乘法。

CSP 周期跟随控制

SDK 的 PDO 周期回调每个总线周期 (125µs) 跑一次。每次回调做两步：

• 先推进虚拟主轴一次。
• 再逐轴算出目标位置并下发。

此外，每个轴都要走 CiA402 使能握手 (0x06 → 0x07 → 0x0F)。握手未完成或出故障时，目标位置恒等于实际位置 (保证无跳变)。

实时控制必须挂在 SDK 的 PDO 周期回调 (ProcessDataCyclicSync) 上，由总线周期 (DC Sync0 125µs) 硬同步驱动。

绝不能用 Thread.Sleep 在用户态自旋。那样会有三个问题：周期不确定、抖动大、与 DC 失同步——不是确定性实时控制。

const int PULSES_PER_REV = 10000;     // STF-EC 默认细分
const int JOG_MASTER_STEP = 20;       // 主轴手动点动每周期步长 (脉冲)

long _masterPos = 0;                  // 虚拟主轴位置 (PDO 线程读写)
volatile int  _masterStep = 10;       // 每周期推进步长 = 速度脉冲/秒 ÷ 8000
volatile bool _syncRunning = false;   // 是否自动推进
volatile bool _resyncRequested = false; // 启动同步 → 下周期重快照 Base + 主轴清零
volatile int  _jogMasterDir = 0;      // 主轴手动点动方向: +1 / -1 / 0

// 实时控制挂到 SDK 的 PDO 周期回调上 —— 由总线周期 (125µs) 硬同步驱动,
// 在 SDK 的 PDO 线程上下文逐周期执行。绝不用 Thread.Sleep / while 自旋。
master.Events.ProcessDataCyclicSync += OnPdoCycle;

/// <summary>
/// PDO 周期回调: SDK 每个总线周期 (LoopCycle = 125µs) 自动回调一次,
/// 推进虚拟主轴 + 各轴按齿比跟随 (125µs 周期, 由 DC 同步驱动)。
/// 参数 mi = 主站序号。
/// </summary>
void OnPdoCycle(ushort mi)
{
    var m = master;
    if (m == null) return;

    var arr = axes;   // 引用快照 (volatile 字段)

    // ① 启动同步: 重快照各轴 Base, 主轴清零 (在 PDO 回调内完成, 保证原子)
    if (_resyncRequested)
    {
        _resyncRequested = false;
        _masterPos = 0;
        for (int i = 0; i < arr.Length; i++)
        {
            ref var input = ref m.Slaves[arr[i].SlaveIndex].PDO.InputsMapping<STF_Input>();
            arr[i].Base          = input.PositionActualValue;   // 以当前实际位置为基准
            arr[i].CurrentTarget = input.PositionActualValue;
        }
    }

    // ② 推进虚拟主轴 (自动同步 + 手动点动)
    if (_syncRunning) _masterPos += _masterStep;
    int jog = _jogMasterDir;
    if (jog != 0) _masterPos += jog * JOG_MASTER_STEP;
    long masterPos = _masterPos;

    // ③ 各轴按齿比跟随同一虚拟主轴
    //    同一回调内连续写各轴目标位置 → 同一 PDO 帧一并提交
    //    (MutexProtection 默认 true, 无需额外加锁)。
    for (int i = 0; i < arr.Length; i++)
    {
        var a = arr[i];
        ref var input  = ref m.Slaves[a.SlaveIndex].PDO.InputsMapping<STF_Input>();
        ref var output = ref m.Slaves[a.SlaveIndex].PDO.OutputsMapping<STF_Output>();
        StepSync(a, masterPos, ref input, ref output);
    }
    // 另有跟随误差/掉OP/驱动故障等报警检测与组停逻辑, 详见源码。
}

/// <summary>
/// 单轴 CSP 步进: CiA402 使能握手 (0x06→0x07→0x0F), 使能后按齿比跟随虚拟主轴。
/// 未使能 / 握手中 / 故障时, 目标位置恒等于实际位置 (无跳变)。
/// </summary>
void StepSync(AxisController a, long masterPos, ref STF_Input input, ref STF_Output output)
{
    output.ModesOfOperation = 8;          // 恒 CSP
    ushort sw = input.StatusWord;
    ushort cw = 0;

    if (a.FaultReset)                      { cw = 0x80; a.FaultReset = false; }  // 故障复位
    else if (IsFault(sw))                  { /* 等待故障复位 */ }
    else if (!a.ServoEnabled)              { a.CurrentTarget = input.PositionActualValue; a.Base = input.PositionActualValue; }
    else if (IsSwitchOnDisabled(sw))       { cw = 0x06; a.CurrentTarget = input.PositionActualValue; a.Base = input.PositionActualValue; } // Shutdown
    else if (IsReadyToSwitchOn(sw))        { cw = 0x07; a.CurrentTarget = input.PositionActualValue; output.TargetPosition = a.CurrentTarget; } // Switch On
    else if (IsSwitchedOn(sw))             { cw = 0x0F; a.CurrentTarget = input.PositionActualValue; output.TargetPosition = a.CurrentTarget; } // Enable Operation
    else if (IsOperationEnabled(sw))
    {
        // —— 同步轴核心: 目标 = 基准 + 主轴位置 × 齿比 ——
        // 齿比 1:1 → 各轴目标增量一致 (同一数据点同时映射); 齿比 ≠ 1 → 电子齿轮。
        a.CurrentTarget = a.Base + (int)Math.Round(masterPos * a.Ratio);
        output.TargetPosition = a.CurrentTarget;
        cw = 0x0F;
    }
    output.ControlWord = cw;
}

位置显示按 PULSES_PER_REV = 10000 换算成角度：角度(°) = 脉冲 × 360 / 10000。 另有报警 / 组停 / 诊断、UI 状态刷新 (≈50ms) 等逻辑，不属于核心控制，详见源码。

操作步骤

1. 准备 config.deni: 用 Darra EtherCAT 主站 GUI 扫描总线上的 STF-EC 从站，导出网络配置为 config.deni。把它放到可执行文件目录下的 EtherCATRestore 文件夹 (可复用 STF-EC_CSP_PP 案例的同一份)。
2. 连接: 点「连接」，等待状态变绿「已连接 (CSP 同步轴)」，总览表列出全部轴。
3. 全部使能: 点「全部使能」，各轴驱动状态进入 OperationEnabled。
4. 设齿比 (可选): 在总览表「齿比」列双击某行直接输入，例如轴 2 填 2.000、轴 3 填 0.500。默认全为 1.000。
5. 启动同步: 设好「速度 (脉冲/秒)」后点「启动同步」。主轴位置开始推进，各轴按齿比一起动起来。齿比都 = 1 时各轴目标位置完全一致；齿比不同时各轴位置按比例拉开。
6. 手动对位 (可选): 用「◀ 主轴-」/「主轴+ ▶」按住点动主轴，观察各轴比例跟随。
7. 停止 / 急停: 「停止」停止主轴推进 (各轴保持当前位置)；「全部去使能 (急停)」立刻松轴。
8. 断开: 点「断开」安全停止并释放主站。