跳到主要内容

智慧楼宇灯光/幕布/LED控制系统

硬件配置​

  • 主站: Windows 10 IoT x 1 + Intel 13900 x 1
  • 运动控制: 松下 MINAS A6B EtherCAT 伺服驱动器 x 120 + 伺服电机 x 120
  • 刹车控制: EL2808 数字量输出 x 2 (12路电磁刹车, 覆盖关键承重轴)

成本分析​

类别型号/规格参考单价 (¥)数量小计 (¥)
伺服驱动器松下 MINAS A6B1,800120216,000
伺服电机200W 伺服电机60012072,000
数字量输出EL280850021,000
工控机Intel 13900 级别10,000110,000

以上为核心部件参考价 (RMB)，不含LED模块、线缆等。

性能指标​

  • 同步精度: ≤1ms
  • 轴数: 120轴同步控制 (6组 x 20轴)
  • PDO 周期: 125μs (8kHz, DC 同步)
  • 位置精度: ±0.1mm

应用场景​

大型演艺场馆异形投影幕布控制系统。120 台伺服分别驱动独立幕布单元 (行程 5m)，按预编排 Cue 列表做分组同步运动，配合 DMX512 灯光联动。12 个关键承重轴配备电磁刹车，掉电默认锁止防坠落。

代码说明​

系统架构​

分组设计​

组数和每组轴数通过配置定义，不限于固定 6x20。示例按物理位置分为 6 组：

组号名称轴范围刹车轴用途
0前幕 (Front)0~190, 10主舞台前排开合幕
1后幕 (Rear)20~3920, 30背景幕布升降
2左侧幕 (Left)40~5940, 50左侧翼幕
3右侧幕 (Right)60~7960, 70右侧翼幕
4天幕 (Ceiling)80~9980, 90顶部天幕/投影幕
5特效幕 (FX)100~119100, 110异形幕/波浪幕
扩展: 动态分组

组定义存储在 JSON 配置文件中，支持任意数量的组和任意轴分配 (不要求连续)。例如:

  • 棋盘分组: 奇数轴一组、偶数轴一组，做交错升降
  • 环形分组: 按圆形舞台角度分 12 组，做旋转波浪
  • 临时分组: 演出中动态创建/合并组
{
  "groups": [
    { "name": "前幕", "axisIds": [0,1,2,...,19], "brakeAxes": [0,10] },
    { "name": "环形-A区", "axisIds": [0,5,10,15,20,25], "brakeAxes": [0,20] }
  ]
}

操作模式与切换​

模式说明操作方式
AUTO自动模式按 Cue 列表顺序执行，操作员按 GO 触发下一个 Cue
MANUAL手动模式选组后点动 (Jog) 或定位 (GoTo)，速度旋钮 0~120%
MAINTENANCE维护模式选单轴低速操作 (限速 100mm/s)，用于调试和校准

Cue 系统​

Cue vs 关键帧: 我们用 CSP 模式在应用层插值，等价实现但更灵活 (可以做任意曲线和波浪效果)。

扩展: Cue 衔接与循环

StageCue 支持 AutoFollowDelay 自动衔接:

  • AutoFollowDelay = -1 — 默认，等待操作员按 GO
  • AutoFollowDelay = 0 — 上一个 Cue 完成后立即执行
  • AutoFollowDelay = 2.0 — 上一个 Cue 完成后延时 2 秒自动执行
  • LoopCount > 1 — 重复执行 (波浪效果循环)

这样可以编排完整的演出序列: 按一次 GO，后续 Cue 按时间轴自动推进。

刹车时序​

关键设计：

  1. 电磁刹车 (失电锁止) — 12 个承重轴配 EL2808 控制，掉电/通信中断时输出归零自动锁止；紧急停止先锁刹车防坠落，正常停止先减速再锁刹车避免冲击
  2. Cue 系统 — 操作员按 GO 触发场景切换，支持自动衔接 (AutoFollowDelay) 和循环 (LoopCount)
  3. 动态分组 — 组数和每组轴分配通过配置定义，支持任意轴组合 (不要求连续)
  4. 异形动作 — PerAxisDelay 支持各轴独立延迟，实现追逐、行波、交错等效果
  5. 手动控制 — 保持式点动 (Jog) 正反向运动、定位 (GoTo)，维护模式限速保护
  6. 速度覆盖 — 全局速度旋钮 0~120%，排练时慢速验证，正式演出恢复原速
  7. 软限位保护 — 安全监控线程检测位置超限 10mm 立即触发急停
  8. 位置反馈 — 每周期从 ActualPosition 同步，防止命令值与实际位置漂移
定制服务

本案例中的 Cue 编排引擎 (插值曲线设计、波浪追逐效果、自动衔接逻辑)、 DMX512 灯光联动协议对接、刹车安全时序逻辑、 以及 120 轴大规模 DC 同步调参等为附加定制服务， 不包含在 EtherCAT 主站软件授权中。

如需演艺场馆舞台自动化方案定制，请联系技术团队。

代码示例​

数据结构​

using System.Runtime.InteropServices;
using System.Collections.Generic;

// ============ PDO 映射 ============

[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct Servo_Input
{
    public ushort StatusWord;    // 0x6041 状态字
    public int ActualPosition;   // 0x6064 实际位置
    public int ActualVelocity;   // 0x606C 实际速度
    public short ActualTorque;   // 0x6077 实际转矩 (0.1%)
}

[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct Servo_Output
{
    public ushort ControlWord;   // 0x6040 控制字
    public int TargetPosition;   // 0x607A 目标位置
    public sbyte ModeOfOperation; // 0x6060 操作模式 (8=CSP)
}

[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct EL2808_Output
{
    public byte DigitalOutputs;  // bit0~7, LOW=刹车锁止, HIGH=释放

    public void SetBit(int ch, bool value)
    {
        if (value) DigitalOutputs |= (byte)(1 << ch);
        else       DigitalOutputs &= (byte)~(1 << ch);
    }
}

// ============ 配置 ============

public class CurtainUnit
{
    public int AxisId, GroupId;
    public double MinPosition, MaxPosition;  // mm
    public int PulsesPerMm;
    public double CurrentPosition;
    public bool HasBrake;
    public int BrakeSlaveIdx, BrakeChannel;
}

public class CurtainGroup
{
    public int GroupId;
    public string Name;
    public int[] AxisIds;                   // 任意轴分配 (不要求连续)
    public double MaxVelocity = 500;        // mm/s
}

// ============ 操作 ============

public enum OpMode { Auto, Manual, Maintenance }
public enum InterpType { Linear, SmoothStep, EaseIn, EaseOut }

public class ManualCommand
{
    public enum CmdType { None, Jog, GoTo }
    public CmdType Type = CmdType.None;
    public int GroupId = -1;      // -1=全部, 0~指定组
    public int AxisId = -1;       // -1=整组 (维护模式用单轴)
    public double JogSpeed;       // mm/s, 正=上升
    public double GoToPosition;   // 目标位置 mm
    public double GoToVelocity;   // 定位速度 mm/s
}

// ============ Cue 系统 ============

public class CueGroupMotion
{
    public int GroupId;
    public double[] Positions;             // 各轴目标位置 mm (长度=组内轴数)
    public double MaxVelocity = 500;       // mm/s
    public InterpType Interp = InterpType.SmoothStep;
    public double[] PerAxisDelay;          // 各轴延迟 (秒), null=同步, 用于追逐/波浪效果
}

/// <summary>
/// 场景 (Cue) — 舞台自动化基本单元
/// </summary>
public class StageCue
{
    public int CueNumber;                  // 编号
    public string Name;                    // 名称 ("开幕", "第一幕", ...)
    public double Duration;                // 过渡时间 (秒)
    public List<CueGroupMotion> Motions;   // 各组运动参数
    public byte[] DmxData;                 // DMX512 灯光, null=不变
    public double AutoFollowDelay = -1;    // -1=等GO, >=0=上个Cue完成后自动延时执行
    public int LoopCount = 1;              // 循环次数 (持续波浪效果)
}

public class CueList
{
    public string ShowName;
    public List<StageCue> Cues = new List<StageCue>();
}
扩展: 效果叠加层

在基础 Cue 运动之上叠加动态调制效果，用于持续性艺术动作:

public enum EffectType { None, Wave, Breathing, Chase, Rainbow }

public class CueEffect
{
    public int GroupId;
    public EffectType Type;
    public double Amplitude;    // 调制幅度 mm
    public double Frequency;    // Hz
    public double PhaseStep;    // 相邻轴相位差 (弧度), Wave 常用 2π/N
}

用法: 在 CueGroupMotion 中增加 CueEffect Effect 字段，ProcessCue 计算基础插值位置后叠加:

double offset = effect.Amplitude * Math.Sin(
    2 * Math.PI * effect.Frequency * elapsed + effect.PhaseStep * axisIndex);
target += offset;

典型效果:

  • Wave: PhaseStep = 2π/20, 整组形成行波
  • Breathing: PhaseStep = 0, 全组同相正弦呼吸
  • Chase: PhaseStep = π, 相邻轴交替升降
  • Rainbow: PhaseStep 递增, 形成螺旋波
扩展: 自定义插值函数

InterpType 枚举可扩展为自定义表达式:

public enum InterpType { Linear, SmoothStep, EaseIn, EaseOut, Bezier, Custom }

public class CueGroupMotion
{
    // ...既有字段...
    public double[] BezierControlPoints;  // Bezier: [cx1, cy1, cx2, cy2]
    public Func<double, double> CustomFunc;  // 自定义: t -> 插值系数
}

支持 Bezier 三次曲线后，可在 UI 中拖拽控制点设计任意加减速曲线，匹配灯光设计师的习惯。

控制代码​

using System;
using System.Diagnostics;
using System.Threading;
using System.Collections.Generic;
using System.Linq;
using DarraEtherCAT_Master;

class CurtainController
{
    const int AXIS_COUNT = 120;
    const int CONTROL_CYCLE_MS = 1;
    const int SLAVE_BRAKE_1 = 120, SLAVE_BRAKE_2 = 121;
    const double MAINTENANCE_MAX_VEL = 100;  // 维护模式限速 mm/s
    static readonly int[] BRAKE_AXES = { 0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110 };

    static CurtainUnit[] Units = new CurtainUnit[AXIS_COUNT];
    static List<CurtainGroup> Groups = new List<CurtainGroup>();

    // 运行状态
    static volatile OpMode mode = OpMode.Auto;
    static volatile bool isRunning = true;         // 正常退出标志
    static volatile bool isEmergencyStop = false;
    static volatile double speedOverride = 1.0;    // 0~1.2
    static int stopDone = 0;

    // Cue 引擎
    static CueList cueList;
    static int cueIndex = -1;
    static volatile bool cueGo = false;
    static volatile bool cueActive = false;        // 当前 Cue 是否正在执行
    static readonly Stopwatch cueTimer = new Stopwatch();
    static double[] cueStartPos = new double[AXIS_COUNT];

    // 手动控制
    static ManualCommand manual = new ManualCommand();
    static volatile long lastJogTick = 0;          // Jog 按键时间戳 (保持式)

    // ======================== 主流程 ========================

    static void Main(string[] args)
    {
        InitGroups();
        InitUnits();

        var master = new DarraEtherCAT()
            .LoadENI("config/smart_lighting.xml")
            .Build();

        if (master == null) { Console.WriteLine("主站初始化失败!"); return; }

        try
        {
            var (ok, msg) = master.SetState(EcState.OP);
            if (!ok) { Console.WriteLine($"无法进入 OP: {msg}"); return; }

            EnableServos(master);

            Console.WriteLine("回原点...");
            HomeAllAxes(master);

            if (!PreShowCheck(master)) { Console.WriteLine("预演检查未通过!"); return; }

            cueList = CueListService.Load("shows/opening_night.json") ?? DefaultCueList();
            Console.WriteLine($"演出 [{cueList.ShowName}] 共 {cueList.Cues.Count} Cue");
            Console.WriteLine("[G]=GO [A]=自动 [M]=手动 [E]=急停 [Q]=退出 [+/-]=速度");

            // 后台线程
            StartThread(() => SafetyLoop(master));
            StartThread(InputLoop);

            // 主控制循环
            while (isRunning && !isEmergencyStop)
            {
                UpdatePositions(master);
                if (mode == OpMode.Auto) ProcessCue(master);
                else                     ProcessManual(master);
                Thread.Sleep(CONTROL_CYCLE_MS);
            }
        }
        finally
        {
            if (isEmergencyStop) EmergencyStop(master);
            else                 GracefulStop(master);
            master.Dispose();
        }
    }

    static void StartThread(ThreadStart action)
    {
        new Thread(action) { IsBackground = true }.Start();
    }

    // ======================== 初始化 ========================

    static void InitGroups()
    {
        // 默认 6 组 x 20 轴 (可从配置文件加载任意分组)
        string[] names = { "前幕", "后幕", "左侧幕", "右侧幕", "天幕", "特效幕" };
        for (int g = 0; g < names.Length; g++)
        {
            int start = g * 20;
            Groups.Add(new CurtainGroup
            {
                GroupId = g, Name = names[g],
                AxisIds = Enumerable.Range(start, 20).ToArray(),
            });
        }
    }

    static void InitUnits()
    {
        for (int i = 0; i < AXIS_COUNT; i++)
        {
            int bi = Array.IndexOf(BRAKE_AXES, i);
            int gid = Groups.FindIndex(g => g.AxisIds.Contains(i));
            Units[i] = new CurtainUnit
            {
                AxisId = i, GroupId = gid >= 0 ? gid : 0,
                MinPosition = 0, MaxPosition = 5000, PulsesPerMm = 100,
                HasBrake = bi >= 0,
                BrakeSlaveIdx = bi >= 0 ? (bi < 8 ? SLAVE_BRAKE_1 : SLAVE_BRAKE_2) : -1,
                BrakeChannel  = bi >= 0 ? (bi < 8 ? bi : bi - 8) : -1,
            };
        }
    }

    static void EnableServos(DarraEtherCAT master)
    {
        // 设置 CSP 操作模式
        for (int i = 0; i < AXIS_COUNT; i++)
        {
            ref var outp = ref master.Slaves[i].OutputsMapping<Servo_Output>();
            outp.ModeOfOperation = 8;  // CSP
            outp.TargetPosition = master.Slaves[i].InputsMapping<Servo_Input>().ActualPosition;
        }
        Thread.Sleep(50);

        // CiA 402: Shutdown -> Switch On -> Enable Operation
        ushort[] sequence = { 0x0006, 0x0007, 0x000F };
        ushort[] statusMasks = { 0x006F, 0x006F, 0x006F };
        ushort[] statusExpects = { 0x0021, 0x0023, 0x0027 };

        for (int s = 0; s < sequence.Length; s++)
        {
            for (int i = 0; i < AXIS_COUNT; i++)
                master.Slaves[i].OutputsMapping<Servo_Output>().ControlWord = sequence[s];

            // 等待所有轴确认状态转换 (超时 2s)
            var deadline = DateTime.Now.AddSeconds(2);
            while (DateTime.Now < deadline)
            {
                bool allReady = true;
                for (int i = 0; i < AXIS_COUNT; i++)
                {
                    var sw = master.Slaves[i].InputsMapping<Servo_Input>().StatusWord;
                    if ((sw & statusMasks[s]) != statusExpects[s]) { allReady = false; break; }
                }
                if (allReady) break;
                Thread.Sleep(10);
            }
        }

        SetBrakes(master, released: true);
        Thread.Sleep(200);
        Console.WriteLine($"已使能 {AXIS_COUNT} 轴 (CSP), 释放 {BRAKE_AXES.Length} 个刹车");
    }

    static bool PreShowCheck(DarraEtherCAT master)
    {
        bool pass = true;
        for (int i = 0; i < AXIS_COUNT; i++)
        {
            var sw = master.Slaves[i].InputsMapping<Servo_Input>().StatusWord;
            if ((sw & 0x006F) != 0x0027)
            {
                var grp = Groups.FirstOrDefault(g => g.AxisIds.Contains(i));
                Console.WriteLine($"  轴{i} ({grp?.Name}) 未使能: 0x{sw:X4}");
                pass = false;
            }
        }
        if (pass) Console.WriteLine("预演检查通过");
        return pass;
    }

    // ======================== Cue 引擎 (自动模式) ========================

    static int cueLoopRemain = 0;                  // 当前 Cue 剩余循环次数
    static readonly Stopwatch autoFollowTimer = new Stopwatch();  // 自动衔接计时

    static void StartCue(int index)
    {
        cueIndex = index;
        var c = cueList.Cues[cueIndex];
        cueLoopRemain = c.LoopCount;

        for (int i = 0; i < AXIS_COUNT; i++)
            cueStartPos[i] = Units[i].CurrentPosition;
        cueTimer.Restart();
        cueActive = true;
        Console.WriteLine($"[GO] Cue {c.CueNumber}: {c.Name} ({c.Duration}s x{c.LoopCount})");
    }

    static void ProcessCue(DarraEtherCAT master)
    {
        // 响应 GO
        if (cueGo)
        {
            cueGo = false;
            autoFollowTimer.Reset();
            if (cueIndex + 1 >= cueList.Cues.Count)
            {
                Console.WriteLine("所有 Cue 已完成, 按 [Q] 退出");
                return;
            }
            StartCue(cueIndex + 1);
        }

        // 自动衔接: 上个 Cue 完成后延时触发下一个
        if (!cueActive && autoFollowTimer.IsRunning && cueIndex + 1 < cueList.Cues.Count)
        {
            var next = cueList.Cues[cueIndex + 1];
            if (next.AutoFollowDelay >= 0 && autoFollowTimer.Elapsed.TotalSeconds >= next.AutoFollowDelay)
            {
                autoFollowTimer.Reset();
                StartCue(cueIndex + 1);
            }
        }

        if (!cueActive || cueIndex < 0) return;

        var cue = cueList.Cues[cueIndex];
        double elapsed = cueTimer.Elapsed.TotalSeconds;
        double t = Math.Min(1.0, elapsed / cue.Duration);

        foreach (var m in cue.Motions)
        {
            var g = Groups[m.GroupId];

            for (int a = 0; a < g.AxisIds.Length; a++)
            {
                int id = g.AxisIds[a];

                // 各轴独立延迟 (追逐/波浪效果)
                double axisT = t;
                if (m.PerAxisDelay != null && a < m.PerAxisDelay.Length)
                {
                    double axisElapsed = elapsed - m.PerAxisDelay[a];
                    axisT = axisElapsed <= 0 ? 0 : Math.Min(1.0, axisElapsed / cue.Duration);
                }

                double it = Interp(axisT, m.Interp);
                double target = Lerp(cueStartPos[id], m.Positions[a], it);
                WriteAxis(master, id, target, m.MaxVelocity * speedOverride);
            }
        }

        // Cue 完成
        if (t >= 1.0 && cueActive)
        {
            // 循环
            if (--cueLoopRemain > 0)
            {
                for (int i = 0; i < AXIS_COUNT; i++)
                    cueStartPos[i] = Units[i].CurrentPosition;
                cueTimer.Restart();
                Console.WriteLine($"[循环] Cue {cue.CueNumber} 剩余 {cueLoopRemain} 次");
                return;
            }

            cueActive = false;
            cueTimer.Stop();

            int next = cueIndex + 1;
            if (next < cueList.Cues.Count)
            {
                var nc = cueList.Cues[next];
                if (nc.AutoFollowDelay >= 0)
                {
                    autoFollowTimer.Restart();
                    Console.WriteLine($"[完成] Cue {cue.CueNumber}, {nc.AutoFollowDelay}s 后自动执行 Cue {nc.CueNumber}");
                }
                else
                    Console.WriteLine($"[完成] Cue {cue.CueNumber}, 按 GO 执行 Cue {nc.CueNumber}: {nc.Name}");
            }
            else
                Console.WriteLine($"[完成] 所有 Cue 执行完毕");
        }
    }

    // ======================== 手动模式 ========================

    static void ProcessManual(DarraEtherCAT master)
    {
        if (manual.Type == ManualCommand.CmdType.None) return;

        if (manual.Type == ManualCommand.CmdType.Jog)
        {
            // 保持式: 200ms 无按键自动停止 (Console 无 key-up, 用超时模拟)
            if (Stopwatch.GetTimestamp() - Interlocked.Read(ref lastJogTick)
                > Stopwatch.Frequency / 5)
            {
                manual.Type = ManualCommand.CmdType.None;
                return;
            }

            double delta = manual.JogSpeed * speedOverride * CONTROL_CYCLE_MS * 0.001;

            // 维护模式: 单轴
            if (mode == OpMode.Maintenance && manual.AxisId >= 0)
            {
                WriteAxis(master, manual.AxisId,
                    Units[manual.AxisId].CurrentPosition + delta, MAINTENANCE_MAX_VEL);
            }
            // 手动模式: 整组
            else if (manual.GroupId >= 0 && manual.GroupId < Groups.Count)
            {
                var g = Groups[manual.GroupId];
                foreach (int id in g.AxisIds)
                    WriteAxis(master, id, Units[id].CurrentPosition + delta, g.MaxVelocity);
            }
        }
        else if (manual.Type == ManualCommand.CmdType.GoTo
                 && manual.GroupId >= 0 && manual.GroupId < Groups.Count)
        {
            var g = Groups[manual.GroupId];
            bool done = true;
            foreach (int id in g.AxisIds)
            {
                WriteAxis(master, id, manual.GoToPosition, manual.GoToVelocity * speedOverride);
                if (Math.Abs(Units[id].CurrentPosition - manual.GoToPosition) > 0.5)
                    done = false;
            }
            if (done)
            {
                Console.WriteLine($"[到位] {g.Name} -> {manual.GoToPosition}mm");
                manual.Type = ManualCommand.CmdType.None;
            }
        }
    }

    // ======================== 位置反馈同步 ========================

    static void UpdatePositions(DarraEtherCAT master)
    {
        for (int i = 0; i < AXIS_COUNT; i++)
            Units[i].CurrentPosition = master.Slaves[i].InputsMapping<Servo_Input>().ActualPosition
                                       / (double)Units[i].PulsesPerMm;
    }

    // ======================== 运动输出 (带速度钳位+软限位) ========================

    static void WriteAxis(DarraEtherCAT master, int id, double target, double maxVel)
    {
        // 速度钳位
        double maxDelta = maxVel * CONTROL_CYCLE_MS * 0.001;
        double delta = target - Units[id].CurrentPosition;
        if (Math.Abs(delta) > maxDelta)
            target = Units[id].CurrentPosition + Math.Sign(delta) * maxDelta;

        // 软限位
        target = Math.Max(Units[id].MinPosition, Math.Min(Units[id].MaxPosition, target));

        ref var output = ref master.Slaves[id].OutputsMapping<Servo_Output>();
        output.TargetPosition = (int)(target * Units[id].PulsesPerMm);
        output.ControlWord = 0x000F;
    }

    // ======================== 回原点 ========================

    static void HomeAllAxes(DarraEtherCAT master)
    {
        // 50mm/s 低速下降触底回零 (50mm/s * 0.001s * 100pulse/mm = 5 pulse/cycle)
        const double HOME_VEL = 50;  // mm/s
        int stepPerCycle = (int)(HOME_VEL * CONTROL_CYCLE_MS * 0.001 * 100);  // 5 pulses
        bool[] done = new bool[AXIS_COUNT];
        int count = 0;

        while (count < AXIS_COUNT)
        {
            for (int i = 0; i < AXIS_COUNT; i++)
            {
                if (done[i]) continue;
                ref var inp = ref master.Slaves[i].InputsMapping<Servo_Input>();
                ref var outp = ref master.Slaves[i].OutputsMapping<Servo_Output>();

                if (Math.Abs(inp.ActualTorque) > 500)
                {
                    outp.TargetPosition = inp.ActualPosition;
                    done[i] = true; count++;
                    Units[i].CurrentPosition = 0;
                }
                else
                {
                    outp.TargetPosition = inp.ActualPosition - stepPerCycle;
                }
                outp.ControlWord = 0x000F;
            }
            Thread.Sleep(CONTROL_CYCLE_MS);
        }
        Console.WriteLine("回原点完成");
    }

    // ======================== 刹车 ========================

    static void SetBrakes(DarraEtherCAT master, bool released)
    {
        foreach (int axis in BRAKE_AXES)
        {
            var u = Units[axis];
            ref var b = ref master.Slaves[u.BrakeSlaveIdx].OutputsMapping<EL2808_Output>();
            b.SetBit(u.BrakeChannel, released);
        }
    }

    // ======================== 安全监控 ========================

    static void SafetyLoop(DarraEtherCAT master)
    {
        while (isRunning && !isEmergencyStop)
        {
            for (int i = 0; i < AXIS_COUNT; i++)
            {
                ref var inp = ref master.Slaves[i].InputsMapping<Servo_Input>();
                string grp = Groups.FirstOrDefault(g => g.AxisIds.Contains(i))?.Name ?? "?";

                if ((inp.StatusWord & 0x0008) != 0)
                {
                    Console.WriteLine($"[故障] 轴{i} ({grp}) 伺服故障 0x{inp.StatusWord:X4}");
                    isEmergencyStop = true; EmergencyStop(master); return;
                }

                double pos = inp.ActualPosition / (double)Units[i].PulsesPerMm;
                if (pos < Units[i].MinPosition - 10 || pos > Units[i].MaxPosition + 10)
                {
                    Console.WriteLine($"[故障] 轴{i} ({grp}) 位置超限 {pos:F1}mm");
                    isEmergencyStop = true; EmergencyStop(master); return;
                }

                if (Math.Abs(inp.ActualTorque) > 900)
                    Console.WriteLine($"[警告] 轴{i} 转矩 {inp.ActualTorque * 0.1:F1}%");
            }
            Thread.Sleep(100);
        }
    }

    // ======================== 停止 ========================

    /// <summary> 紧急停止: 锁刹车(防坠落) -> Quick Stop -> 等减速 -> 断电 </summary>
    static void EmergencyStop(DarraEtherCAT master)
    {
        if (Interlocked.Exchange(ref stopDone, 1) == 1) return;
        Console.WriteLine("[紧急停止]");
        SetBrakes(master, released: false);
        SetAllControlWord(master, 0x000B);  // Quick Stop
        WaitStopped(master, 2000);
        SetAllControlWord(master, 0x0000);  // Disable Voltage
        Console.WriteLine($"已停止, {BRAKE_AXES.Length} 个刹车锁止");
    }

    /// <summary> 正常停止: 锁位 -> 等减速 -> 锁刹车 -> 断电 </summary>
    static void GracefulStop(DarraEtherCAT master)
    {
        if (Interlocked.Exchange(ref stopDone, 1) == 1) return;
        Console.WriteLine("[正常停止]");
        for (int i = 0; i < AXIS_COUNT; i++)
        {
            ref var outp = ref master.Slaves[i].OutputsMapping<Servo_Output>();
            outp.TargetPosition = master.Slaves[i].InputsMapping<Servo_Input>().ActualPosition;
            outp.ControlWord = 0x000F;
        }
        WaitStopped(master, 5000);
        SetBrakes(master, released: false);
        Thread.Sleep(50);
        SetAllControlWord(master, 0x0000);
        Console.WriteLine($"已停止, {BRAKE_AXES.Length} 个刹车锁止");
    }

    static void SetAllControlWord(DarraEtherCAT master, ushort cw)
    {
        for (int i = 0; i < AXIS_COUNT; i++)
            master.Slaves[i].OutputsMapping<Servo_Output>().ControlWord = cw;
    }

    static void WaitStopped(DarraEtherCAT master, int timeoutMs)
    {
        var deadline = DateTime.Now.AddMilliseconds(timeoutMs);
        while (DateTime.Now < deadline)
        {
            bool stopped = true;
            for (int i = 0; i < AXIS_COUNT; i++)
                if (Math.Abs(master.Slaves[i].InputsMapping<Servo_Input>().ActualVelocity) > 10)
                { stopped = false; break; }
            if (stopped) return;
            Thread.Sleep(CONTROL_CYCLE_MS);
        }
        Console.WriteLine("[警告] 减速超时");
    }

    // ======================== 操作员输入 ========================

    static void InputLoop()
    {
        while (isRunning && !isEmergencyStop)
        {
            if (!Console.KeyAvailable) { Thread.Sleep(50); continue; }
            var key = Console.ReadKey(true).Key;

            switch (key)
            {
                case ConsoleKey.G:
                    if (mode == OpMode.Auto) cueGo = true;
                    break;
                case ConsoleKey.Q:
                    isRunning = false;
                    Console.WriteLine("[正常退出]");
                    break;
                case ConsoleKey.A:
                    mode = OpMode.Auto; manual.Type = ManualCommand.CmdType.None;
                    Console.WriteLine("[自动模式]");
                    break;
                case ConsoleKey.M:
                    mode = OpMode.Manual;
                    Console.WriteLine("[手动模式] 数字键选组, 方向键点动");
                    break;
                case ConsoleKey.T:
                    mode = OpMode.Maintenance;
                    Console.WriteLine("[维护模式] 限速 100mm/s");
                    break;
                case ConsoleKey.E:
                    isEmergencyStop = true; break;

                // 速度覆盖
                case ConsoleKey.OemPlus: case ConsoleKey.Add:
                    speedOverride = Math.Min(1.2, speedOverride + 0.1);
                    Console.WriteLine($"[速度] {speedOverride * 100:F0}%");
                    break;
                case ConsoleKey.OemMinus: case ConsoleKey.Subtract:
                    speedOverride = Math.Max(0, speedOverride - 0.1);
                    Console.WriteLine($"[速度] {speedOverride * 100:F0}%");
                    break;

                // 选组 (数字键 0~9)
                case ConsoleKey.D0: case ConsoleKey.D1: case ConsoleKey.D2:
                case ConsoleKey.D3: case ConsoleKey.D4: case ConsoleKey.D5:
                case ConsoleKey.D6: case ConsoleKey.D7: case ConsoleKey.D8:
                case ConsoleKey.D9:
                    if (mode != OpMode.Auto)
                    {
                        int gid = key - ConsoleKey.D0;
                        if (gid < Groups.Count)
                        {
                            manual.GroupId = gid;
                            Console.WriteLine($"[选组] {Groups[gid].Name}");
                        }
                    }
                    break;

                // 点动 (保持按键刷新时间戳, 200ms 无按键自动停止)
                case ConsoleKey.UpArrow:
                    if (mode != OpMode.Auto)
                    {
                        Interlocked.Exchange(ref lastJogTick, Stopwatch.GetTimestamp());
                        manual.Type = ManualCommand.CmdType.Jog;
                        manual.JogSpeed = mode == OpMode.Maintenance ? MAINTENANCE_MAX_VEL : 300;
                    }
                    break;
                case ConsoleKey.DownArrow:
                    if (mode != OpMode.Auto)
                    {
                        Interlocked.Exchange(ref lastJogTick, Stopwatch.GetTimestamp());
                        manual.Type = ManualCommand.CmdType.Jog;
                        manual.JogSpeed = -(mode == OpMode.Maintenance ? MAINTENANCE_MAX_VEL : 300);
                    }
                    break;
            }
        }
    }

    // ======================== 默认 Cue 列表 ========================

    static CueList DefaultCueList()
    {
        var list = new CueList { ShowName = "默认演出" };
        int axesPerGroup = Groups[0].AxisIds.Length;

        // Cue 1: 开幕 — 前幕升顶 (操作员按 GO 触发)
        list.Cues.Add(new StageCue
        {
            CueNumber = 1, Name = "开幕", Duration = 5.0,
            Motions = new List<CueGroupMotion>
            {
                new CueGroupMotion
                {
                    GroupId = 0,
                    Positions = Enumerable.Repeat(5000.0, axesPerGroup).ToArray(),
                    MaxVelocity = 300, Interp = InterpType.EaseIn,
                },
            },
        });

        // Cue 2: 第一幕 — 后幕下降 + 天幕展开 (Cue 1 完成后 2s 自动执行)
        list.Cues.Add(new StageCue
        {
            CueNumber = 2, Name = "第一幕", Duration = 8.0,
            AutoFollowDelay = 2.0,
            Motions = new List<CueGroupMotion>
            {
                new CueGroupMotion { GroupId = 1,
                    Positions = Enumerable.Repeat(1000.0, axesPerGroup).ToArray(),
                    MaxVelocity = 200, Interp = InterpType.SmoothStep },
                new CueGroupMotion { GroupId = 4,
                    Positions = Enumerable.Repeat(3000.0, axesPerGroup).ToArray(),
                    MaxVelocity = 400, Interp = InterpType.SmoothStep },
            },
        });

        // Cue 3: 追逐波浪 — 特效幕正弦波 + 各轴延迟形成行波, 循环 3 次
        var wave = new double[axesPerGroup];
        var delays = new double[axesPerGroup];
        for (int i = 0; i < axesPerGroup; i++)
        {
            wave[i] = 2500 + 2000 * Math.Sin(2 * Math.PI * i / axesPerGroup);
            delays[i] = i * 0.15;  // 每轴延迟 150ms, 形成行波追逐效果
        }

        list.Cues.Add(new StageCue
        {
            CueNumber = 3, Name = "追逐波浪", Duration = 4.0,
            AutoFollowDelay = 0,  // Cue 2 完成后立即执行
            LoopCount = 3,
            Motions = new List<CueGroupMotion>
            {
                new CueGroupMotion { GroupId = 5,
                    Positions = wave, MaxVelocity = 500,
                    Interp = InterpType.SmoothStep,
                    PerAxisDelay = delays },
            },
        });

        // Cue 4: 谢幕 — 所有幕回零位 (按 GO 触发)
        list.Cues.Add(new StageCue
        {
            CueNumber = 4, Name = "谢幕", Duration = 10.0,
            Motions = Groups.Select(g => new CueGroupMotion
            {
                GroupId = g.GroupId,
                Positions = Enumerable.Repeat(0.0, g.AxisIds.Length).ToArray(),
                MaxVelocity = 200, Interp = InterpType.EaseOut,
            }).ToList(),
        });

        return list;
    }

    // ======================== 辅助 ========================

    static double Lerp(double a, double b, double t) => a + (b - a) * t;

    static double Interp(double t, InterpType type) => type switch
    {
        InterpType.Linear     => t,
        InterpType.SmoothStep => t * t * (3 - 2 * t),
        InterpType.EaseIn     => t * t,
        InterpType.EaseOut    => 1 - (1 - t) * (1 - t),
        _                     => t * t * (3 - 2 * t),
    };
}