龙门双驱差动

龙门两侧各一台伺服，按 1:1 走同一条位置指令。在 ProcessDataCyclicSync 里读两轴实际位置、下发同一条 CSP 指令，并用跟随误差做轻微纠偏。同一周期写完两轴。

硬件配置

• 主站: Windows 10/11 × 1，DarraRT 实时驱动
• 伺服: CiA 402 伺服 × 2（龙门左/右立柱，下文 Y1 / Y2）
• 模式: CSP（周期同步位置, 0x6060 = 8）
• 同步: DC Sync0 = 125µs，与 LoopCycle 一致

轴映射、DC、进 OP 的步骤与 同步轴 / 电子齿轮 相同。本页只写差动周期回调。

性能指标

• 控制周期: 125µs
• 两轴目标在同一帧发出
• 纠偏只用当周期跟随误差，不做跨周期规划

工作原理

公共指令 P
Y1 目标 = P
Y2 目标 = P + clamp(k × (Y1实际 − Y2实际), ±Emax)

k 取很小的整数增益（例如 1/16 用位移实现），Emax 限制单周期纠偏，避免振荡。齿比不是 1:1 时，用 电子齿轮；曲线跟随用 电子凸轮。

代码示例

using System.Runtime.InteropServices;
using DarraEtherCAT_Master;

[StructLayout(LayoutKind.Sequential, Pack = 1)]
struct ServoIn
{
    public ushort StatusWord;
    public int ActualPosition;
}

[StructLayout(LayoutKind.Sequential, Pack = 1)]
struct ServoOut
{
    public ushort ControlWord;
    public sbyte Mode;
    public int TargetPosition;
}

sealed class DualDrive
{
    const int Y1 = 1, Y2 = 2;
    const int Emax = 40;
    const int Enable = 0x000F;

    readonly DarraEtherCAT _master;
    int _command;

    public DualDrive(DarraEtherCAT master)
    {
        _master = master;
        master.Events.ProcessDataCyclicSync += OnPdo;
    }

    public void Jog(int deltaPulses) => _command += deltaPulses;

    void OnPdo(ushort masterIndex)
    {
        ref var y1In  = ref _master.Slaves[Y1].PDO.InputsMapping<ServoIn>();
        ref var y2In  = ref _master.Slaves[Y2].PDO.InputsMapping<ServoIn>();
        ref var y1Out = ref _master.Slaves[Y1].PDO.OutputsMapping<ServoOut>();
        ref var y2Out = ref _master.Slaves[Y2].PDO.OutputsMapping<ServoOut>();

        int err = y1In.ActualPosition - y2In.ActualPosition;
        int trim = err / 16;
        if (trim > Emax) trim = Emax;
        if (trim < -Emax) trim = -Emax;

        y1Out.ControlWord = Enable;
        y1Out.Mode = 8;
        y1Out.TargetPosition = _command;

        y2Out.ControlWord = Enable;
        y2Out.Mode = 8;
        y2Out.TargetPosition = _command + trim;
    }
}

进 OP 之前把两轴 TargetPosition 设成当时的实际位置。多轴齿比用 同步轴 / 电子齿轮，凸轮用 多轴电子凸轮。