多轴电子凸轮

一根虚拟主轴驱动多根从轴，每根从轴按设定的凸轮曲线实时跟随主轴相位运动。这就是包装、印刷、飞剪、灌装等场合里机械凸轮的电子化替代。

本案例的几个要点：

• 主轴是软件相位，不用真实编码器：用 masterPhase ∈ [0,1) 当主轴，每个 PDO 周期 (125µs) 推进一次。
• 从轴只用 CSP 模式 (Mode=8)：周期同步位置模式，跟随主站逐周期算出的凸轮目标位置。

完整源码

本案例完整源码 (WinForms + CiA 402 状态机 + 虚拟主轴 + 凸轮曲线 + CSP 跟随线程 + 报警/诊断) 见 GitHub 仓库： https://github.com/DarraTechnology/Ethercat_Master/tree/main/Windows/CSharp/STF-EC_ECam

硬件配置

• 主站: Windows 10 × 1 + Intel i5 × 1
• 步进驱动器: STF-EC EtherCAT 步进驱动器 × 5 (厂商 鸣志 Shanghai AMP&MOONS' Automation, VendorId 0x00000168, ProductCode 0x02)
• 协议: CoE (CiA 402, Profile 402)，DC Sync0 125µs

项	值
厂商	Shanghai AMP&MOONS' Automation (鸣志)
VendorId	0x00000168
型号 / ProductCode	STF-EC / 0x02
轴数	随附 config.deni 含 5 轴 (实际以扫描为准, master.SlaveCount)
控制模式	CSP 周期同步位置 (Mode = 8)
同步	DC Sync0 125µs (CSP 必需)

轴数由 config.deni 实际扫描到的从站数量决定，界面按轴数自动生成总览表，无需改代码。5 轴可全部当作凸轮从轴。

运动参数 / 性能指标

• 控制周期: 125µs (LoopCycle = DC Sync0, 硬同步)
• 虚拟主轴: 软件相位 masterPhase ∈ [0,1)，主轴转速可调 (RPM)
• 凸轮曲线: 正弦 / 摆线 / 直线 (电子齿轮) 三种，行程 (脉冲) 可调
• 相位偏移: 每轴独立设定，多轴可在凸轮上均匀错开 (如 5 轴各偏 0/72/144/216/288°)
• 挂载无跳变: 挂载一刻快照实际位置作基准 Base，跟随过程不产生位置阶跃
• 跟随误差阈值随凸轮速度自适应，避免高速/大行程误报

应用场景

机械凸轮机构的电子化替代，典型场合：

• 包装机: 走膜、封切、横封纵封等动作按主轴 (输送链) 相位做确定位移
• 印刷机: 各印刷单元色组随版辊相位精确套准
• 飞剪 / 追剪: 剪刀按料带速度同步加减速，剪切瞬间与料带等速
• 灌装机: 灌装头随转盘相位升降、开合阀

相比机械凸轮，电子凸轮换曲线只需改软件参数 (无需更换实体凸轮盘)，且可在线调整行程、相位、主轴转速。

工作原理

先看机械凸轮：主轴每转一圈，从动件就按凸轮轮廓做一段确定位移。也就是说，从动件位移是主轴转角的函数 s = f(θ)。

电子凸轮就是把这条轮廓做成数学曲线，由控制器每周期实时计算并下发。本案例围绕四个关键概念：

1. 虚拟主轴相位推进：用软件相位 masterPhase ∈ [0,1) 代替真实编码器主轴，每个 PDO 周期 (125µs) 推进

   Δphase = 主轴RPM / 60 / 8000   // 8000 Hz = 125µs 周期
   masterPhase = frac(masterPhase + Δphase)   // 到 1 自动回 0 循环, 一圈 = 0→360°

2. 挂载 (Engage) 无跳变：从轴勾选"挂载"才跟随主轴。挂载一刻快照当前实际位置作基准 Base，从轴目标 = Base + Cam(...) × 行程，所以挂载/启动时从轴目标恰好等于它当前所在位置，无位置阶跃。
3. 相位偏移 (Phase Offset)：每根从轴可设各自相位偏移 (度)，让多轴在凸轮上错开 —— 例如 5 轴各偏 0/72/144/216/288°，即在一个周期内均匀分布。
4. 从轴目标位置公式 (每周期、每挂载轴)：

   output.TargetPosition = Base + (int)Round( Cam(曲线, frac(masterPhase + 相位偏移/360)) × 行程 )

为什么只用 CSP (Mode = 8)？ 一句话：电子凸轮要求每个控制周期都精确下发一个新目标位置，并与总线周期硬同步——这正好是 CSP 的语义。

CSP 的具体工作方式：

• 主站每周期把算好的凸轮目标位置写入 0x607A，轨迹由主站算，驱动器只做位置跟随。
• 配合 DC Sync0 (125µs) 把各从站时钟对齐到同一节拍，多轴凸轮才不会彼此漂移。

为什么不用 PP？ PP (轮廓位置) 是点到点定位，由驱动器内部自己生成轨迹。主站给不了逐周期的凸轮形状，所以 PP 不适合凸轮，本例不提供 PP 分支。

系统架构

主站职责很轻，每周期只做三件事：

1. 推进虚拟主轴相位；
2. 对每根挂载从轴，用它的相位偏移取曲线值，算出目标位置；
3. 维持 CiA 402 使能握手，把目标位置写进 RxPDO。

它不做内部插补、也不做轨迹规划——凸轮形状完全由曲线函数决定。

代码示例

PDO 结构体定义

STF-EC 默认 RxPDO 输出 29 字节、TxPDO 输入 35 字节。注意 0x1A00 中错误码在前、状态字在后，结构体 STF_Input 已照此排列。

using System.Runtime.InteropServices;

/// <summary>
/// STF-EC 步进驱动器输出 PDO (RxPDO 0x1600+0x1601+0x1602+0x1603, 共 29 字节)
/// 电子凸轮只用到 ControlWord / ModesOfOperation / TargetPosition 三项,
/// 其余字段保留以匹配 config.deni 的完整 PDO 分配 (尺寸必须严格对齐)。
/// </summary>
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct STF_Output  // 29 字节
{
    // 0x1600 —— 凸轮跟随实际用到的三项
    public ushort ControlWord;          // 0x6040:0  控制字 (CiA402 状态机)
    public sbyte ModesOfOperation;      // 0x6060:0  操作模式 (CSP = 8)
    public int TargetPosition;          // 0x607A:0  目标位置 (脉冲, 每周期由凸轮算出)
    // 0x1601 —— 保留 (CSP 用不到, 但 PDO 分配里有, 必须占位)
    public uint ProfileVelocity;        // 0x6081:0
    public uint ProfileAcceleration;    // 0x6083:0
    public uint ProfileDeceleration;    // 0x6084:0
    // 0x1602
    public int TargetVelocity;          // 0x60FF:0
    // 0x1603
    public uint PhysicalOutputs;        // 0x60FE:1  数字输出
    public ushort TouchProbeFunction;   // 0x60B8:0
}

/// <summary>
/// STF-EC 步进驱动器输入 PDO (TxPDO 0x1A00+0x1A01+0x1A02+0x1A03, 共 35 字节)
/// </summary>
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct STF_Input  // 35 字节
{
    // 0x1A00  (注意: STF-EC 此 PDO 错误码在前, 状态字在后)
    public ushort ErrorCode;                // 0x603F:0  错误码 (≠0 表示驱动故障)
    public ushort StatusWord;               // 0x6041:0  状态字 (判 CiA402 状态机)
    public sbyte ModesOfOperationDisplay;   // 0x6061:0
    // 0x1A01
    public int PositionActualValue;         // 0x6064:0  实际位置 (脉冲, 挂载时快照作 Base)
    // 0x1A02
    public int VelocityActualValue;         // 0x606C:0  实际速度
    // 0x1A03
    public uint DigitalInputs;              // 0x60FD:0
    public ushort TouchProbeStatus;         // 0x60B9:0
    public int TouchProbe1PosValue;         // 0x60BA:0
    public int TouchProbe1NegValue;         // 0x60BB:0
    public int TouchProbe2PosValue;         // 0x60BC:0
    public int TouchProbe2NegValue;         // 0x60BD:0
}

连接与 DC 同步

连接流程为 Build → PreOp → (逐轴 PDO 重映 + DC) → SafeOp → (尺寸自检) → OP。

其中 PDO 重映 (0x1C12/0x1C13) 与 DC Sync0 是 CSP 凸轮的两个必备前提：

• 不重映：驱动默认 RxPDO 字节数和结构体对不上。
• 不开 DC：多轴时钟会漂移。

下面只摘关键步骤，省略了 try/catch 与取消检查这类样板。

// 构建主站并扫描从站 (EnableAutoStartup 自动跑启动指令)
var buildResult = new DarraEtherCAT()
    .SetENI(deniPath)            // 加载 config.deni (带 SHA256 校验的真实拓扑)
    .EnableAutoStartup()
    .Build();
if (!buildResult.Success) { /* 初始化失败处理 */ return; }

var master = buildResult.Master;
int slaveCount = master.SlaveCount;   // 轴数由实际扫描决定

// 进 PreOp 后才能写 SDO 重映 PDO 分配
master.SetState(EcState.PreOp);

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

    // ① 经 CoE 把 PDO 分配重映成 config.deni 的完整 4+4 (输出 29 / 输入 35)。
    //    驱动默认 RxPDO 较短, 不重映就和结构体对不上, 任何模式都映射失败。
    //    这是一次性初始化 (PreOp 态写, SM2 此时未激活), 不进控制循环。
    slave.CoE.SDOWrite(0x1C12, 0, new byte[] { 0 });                        // 先清空 RxPDO 分配数
    slave.CoE.SDOWrite(0x1C12, 1, BitConverter.GetBytes((ushort)0x1600));
    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 个 RxPDO
    slave.CoE.SDOWrite(0x1C13, 0, new byte[] { 0 });                        // 清空 TxPDO 分配
    slave.CoE.SDOWrite(0x1C13, 1, BitConverter.GetBytes((ushort)0x1A00));
    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 个 TxPDO

    // ② 启用 DC Sync0 = 125µs (125000 ns), 把各从站时钟对齐到同一节拍 —— CSP 凸轮必需
    //    SYNC0 与 master.Config.LoopCycle 必须保持一致, 否则 PDO 帧与从站节拍失同步
    master.Config.LoopCycle = 125000;   // PDO 交换周期 = 125µs (Windows 推荐最小周期)
    if (slave.HasDC) slave.ConfigureDC(125000);   // Sync0 = 125µs
    slave.CoE?.SDOWrite(0x10F1, 2, BitConverter.GetBytes((ushort)65535));   // 放宽同步误差容限
}

master.SetState(EcState.SafeOp);

// ③ 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)
        { /* 驱动当前 PDO 映射 ≠ config.deni, 报错 */ return; }
}

// ④ 进 OP 前: 目标位置先对齐当前实际位置, 上电不跳变
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;  // 目标 = 当前实际, 无运动
}

master.SetState(EcState.OP);   // 进 OP, PDO 控制线程接管

凸轮曲线 Cam()

凸轮曲线是电子凸轮的灵魂。Cam(type, u) 输入主轴相位 u ∈ [0,1)，返回归一化位移 [-1,1]，乘以行程 (脉冲) 即得实际位置偏移。三种曲线对应三类典型机械凸轮轮廓：

// 取小数部分: 把任意相位规整到 [0,1) 一圈内
static double Frac(double x) => x - Math.Floor(x);

/// <summary>
/// 凸轮曲线: u ∈ [0,1) 为主轴相位, 返回 [-1,1] 归一化位移。
/// </summary>
public static double Cam(int type, double u)
{
    u = Frac(u);   // 相位规整到 [0,1)
    switch (type)
    {
        case 0: // 正弦 Sine: 一圈内一个完整正弦往复, 加减速平滑, 最常用
            return Math.Sin(2.0 * Math.PI * u);

        case 1: // 摆线 Cycloidal: 起停加速度连续(无冲击), 高速凸轮首选
        {
            double rise;
            if (u < 0.5)
            {
                double s = u * 2.0;                                            // s ∈ [0,1)
                rise = s - Math.Sin(2.0 * Math.PI * s) / (2.0 * Math.PI);      // 平滑上升 0→1
            }
            else
            {
                double s = (u - 0.5) * 2.0;                                    // s ∈ [0,1)
                double up = s - Math.Sin(2.0 * Math.PI * s) / (2.0 * Math.PI);
                rise = 1.0 - up;                                              // 镜像回落 1→0
            }
            return rise * 2.0 - 1.0;                                          // 0..1 映射到 -1..1
        }

        case 2: // 直线 / 电子齿轮: 线性斜坡, 位移与主轴相位严格成正比
            return u * 2.0 - 1.0;

        default:
            return 0.0;
    }
}

关键洞见：同步轴 (电子齿轮) 就是凸轮曲线取直线时的线性特例。

当 type = 2 (直线) 时，从轴位移 2u-1 与主轴相位严格成比例，这正是电子齿轮 / 同步轴的 1:1 比例跟随。

换句话说，"电子齿轮"只是凸轮曲线里最简单的一根直线。二者本质同源：凸轮是一般的 s = f(θ)，齿轮则是其中 f 取线性的那个特例。

CSP 周期跟随控制

凸轮控制挂在 SDK 的 PDO 周期回调上，每个总线周期 (125µs) 执行一次，做三件事：

1. 凸轮启动时锁存各挂载轴的 Base；
2. 推进虚拟主轴相位；
3. 逐轴调用 StepCam，做 CiA 402 握手并算凸轮目标位置。

此外还有报警检测 / 跟随误差判断逻辑：PDO 热路径只置 volatile 闩，真正的决策与呈现放在 50ms 的 UI 消费侧。这部分此处省略，详见源码。

实时控制必须挂在 SDK 的 PDO 周期回调 (ProcessDataCyclicSync) 上, 由总线周期 (DC Sync0 125µs) 硬同步驱动。绝不能用 Thread.Sleep 在用户态自旋——那样周期不确定、抖动大、与 DC 失同步, 不是确定性实时控制。

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

// PDO 周期回调: SDK 每个总线周期 (LoopCycle = 125µs) 自动回调一次,
// 推进主轴相位 + 逐轴凸轮跟随。参数 mi = 主站序号。
void OnPdoCycle(ushort mi)
{
    var m = master;
    if (m == null) return;

    var arr = axes;
    _pdoCycle++;

    // ① 凸轮启动锁存: 快照各挂载轴当前实际位置作为 Base, 跟随时无跳变
    if (_camStartLatch)
    {
        for (int i = 0; i < arr.Length; i++)
        {
            int actual = m.Slaves[arr[i].SlaveIndex]
                .PDO.InputsMapping<STF_Input>().PositionActualValue;
            arr[i].Base = actual;           // 快照当前实际位置
        }
        if (Interlocked.Exchange(ref _masterPhaseResetFlag, 0) == 1)
            _masterPhase = 0.0;             // 相位归零 (可选)
        _camStartLatch = false;
    }

    // ② 主轴相位推进: Δphase = rpm/60/8000 (8000 Hz = 125µs 周期), 到 1 自动回 0
    if (_camRunning)
        _masterPhase = Frac(_masterPhase + _masterRpm / 60.0 / 8000.0);

    int curve   = _curveType;
    int amp     = _amplitude;        // 行程 (脉冲)
    double phase = _masterPhase;

    // ③ 逐轴 CSP 跟随 —— 同一回调内连续写各轴目标位置, 同一 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>();
        StepCam(a, ref input, ref output, curve, amp, phase);
        // ... 另有报警/跟随误差检测, 详见源码 ...
    }
}

// 单轴 CSP 凸轮跟随: 先维持 CiA402 使能握手 (0x06→0x07→0x0F, 故障 0x80),
// 进入 OperationEnabled 后按凸轮算目标位置写入 RxPDO。
void StepCam(AxisController a, ref STF_Input input, ref STF_Output output,
             int curve, int amp, double phase)
{
    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)       { output.TargetPosition = input.PositionActualValue; }
    else if (IsSwitchOnDisabled(sw)){ cw = 0x06; output.TargetPosition = input.PositionActualValue; }  // Shutdown
    else if (IsReadyToSwitchOn(sw)) { cw = 0x07; output.TargetPosition = input.PositionActualValue; }  // Switch On
    else if (IsSwitchedOn(sw))      { cw = 0x0F; output.TargetPosition = input.PositionActualValue; }  // Enable Operation
    else if (IsOperationEnabled(sw))
    {
        cw = 0x0F;   // 保持使能
        if (a.Engaged && _camRunning)
        {
            // ★ 核心: 从轴目标 = Base + Cam(曲线, 主轴相位 + 该轴相位偏移) × 行程
            double u = Frac(phase + a.PhaseOffsetDeg / 360.0);   // 加相位偏移让多轴错开
            int target = a.Base + (int)Math.Round(Cam(curve, u) * amp);
            output.TargetPosition = target;
        }
        else
        {
            // 未挂载 / 凸轮未运行: 保持当前实际位置 (无运动)
            output.TargetPosition = input.PositionActualValue;
        }
    }
    output.ControlWord = cw;
}

CiA 402 状态机的判别，都是对状态字 sw 做掩码比较：

• IsSwitchOnDisabled = (sw & 0x6F) == 0x40
• IsReadyToSwitchOn = 0x21
• IsSwitchedOn = 0x23
• IsOperationEnabled = (sw & 0x6F) == 0x27
• IsFault = (sw & 0x4F) == 0x08

使能握手依次发控制字 0x06 → 0x07 → 0x0F；遇到故障则发 0x80 复位。

操作步骤

1. 连接: 走 Init → PreOp → SafeOp → OP，逐轴启用 DC Sync0、PDO 重映与尺寸自检，进入 CSP。
2. 全部使能: 各轴跑 CiA 402 握手 (0x06 → 0x07 → 0x0F) 进入 OperationEnabled。
3. 选曲线 + 行程: 选凸轮曲线 (正弦 / 摆线 / 直线) 和行程脉冲；界面即时预览曲线形状。
4. (可选) 设相位偏移: 给各轴填不同角度，让多轴在凸轮上均匀错开 (如 0/72/144/216/288°)。
5. 全部挂载: 把从轴挂到凸轮 —— 挂载一刻快照 Base，跟随无跳变。
6. 启动凸轮: 设主轴 RPM 后启动，主轴相位开始推进，挂载轴按曲线跟随；停止让各轴停在当前位置，相位归零把主轴相位与基准重置 (无跳变)。

安全约定

连接/上电后不会自动运动——因为目标位置初始化成了当前实际位置。必须先「使能」→「挂载」→「启动凸轮」，电机才会动。

此外还有几类诊断逻辑，详见源码：

• 报警分级 (故障 / 警告 / 信息)；
• 故障组停闭锁；
• 跟随误差防误报 (grace 宽限)。