TeamAAS-2.5/TeamAAS.Plugins/Plugins.Motion/MotionMovePlugins.cs

using System;
using System.Collections.Generic;
using System.Threading;
using Plugins.Motion.Models;
using TeamAAS.FlowEditor.Models;
using TeamAAS.FlowEditor.Plugins;
using TeamAAS.Motion;

namespace Plugins.Motion
{
    [Serializable]
    [Plugin("轴回零", PluginCategory.硬件模块, typeof(MotionHomeModel), "Home",
        Description = "指定轴回零，可勾选未使能时自动使能，并等待回零完成")]
    public class MotionHomePlugin : BasePlugin<MotionHomeModel>
    {
        public override List<OutputField> DeclareOutputs() => new List<OutputField>
        {
            new OutputField("结果", typeof(bool)),
            new OutputField("已回零", typeof(bool)),
            new OutputField("最终位置", typeof(double)),
        };

        public override NodeRunStatus PluginRun(CancellationToken token, out Dictionary<string, object> results)
        {
            var res = new Dictionary<string, object>();
            results = res;
            NodeRunStatus Fail(string m)
            {
                res["Error"] = m;
                res["结果"] = false;
                Log(1, $"轴回零失败: {m}", TeamAAS.LogLevel.Error);
                return NodeRunStatus.Failed;
            }

            try
            {
                if (Model.AxisNo < 0) return Fail("回零必须指定具体轴号（不能为 -1）");

                var card = MotionPluginHelper.GetCard(Service<MotionManager>(), Model.MotionCardName, out var err);
                if (card == null) return Fail(err);
                err = MotionPluginHelper.EnsureConnected(card, Model.AutoConnect);
                if (err != null) return Fail(err);
                err = MotionPluginHelper.EnsureEnabled(card, Model.AxisNo, Model.AutoEnable);
                if (err != null) return Fail(err);
                err = MotionPluginHelper.CheckMotionAllowed(card);
                if (err != null) return Fail(err);

                Log(3, $"轴回零 卡={card.Name} 轴={Model.AxisNo} 模式={Model.HomeMode}");
                if (!card.Home(Model.AxisNo, Model.HomeMode))
                    return Fail("下发回零失败");

                err = MotionPluginHelper.WaitIdle(card, Model.AxisNo, Model.WaitTimeoutMs, token);
                if (err != null) return Fail(err);

                var st = card.GetAxisStatus(Model.AxisNo);
                MotionPluginHelper.FillStatus(res, st);
                if (st == null || !st.IsHomed)
                    return Fail("回零结束但轴未处于已回零状态");

                res["结果"] = true;
                return NodeRunStatus.Success;
            }
            catch (OperationCanceledException) { throw; }
            catch (Exception ex) { return Fail(ex.Message); }
        }
    }

    [Serializable]
    [Plugin("绝对定位", PluginCategory.硬件模块, typeof(MotionMoveAbsModel), "AxisArrow",
        Description = "单轴绝对定位，目标位置/速度可公式绑定，可勾选未使能时自动使能")]
    public class MotionMoveAbsPlugin : BasePlugin<MotionMoveAbsModel>
    {
        public override List<OutputField> DeclareOutputs() => new List<OutputField>
        {
            new OutputField("结果", typeof(bool)),
            new OutputField("最终位置", typeof(double)),
            new OutputField("目标位置", typeof(double)),
        };

        public override NodeRunStatus PluginRun(CancellationToken token, out Dictionary<string, object> results)
        {
            var res = new Dictionary<string, object>();
            results = res;
            NodeRunStatus Fail(string m)
            {
                res["Error"] = m;
                res["结果"] = false;
                Log(1, $"绝对定位失败: {m}", TeamAAS.LogLevel.Error);
                return NodeRunStatus.Failed;
            }

            try
            {
                if (Model.AxisNo < 0) return Fail("定位必须指定具体轴号（不能为 -1）");

                var card = MotionPluginHelper.GetCard(Service<MotionManager>(), Model.MotionCardName, out var err);
                if (card == null) return Fail(err);
                err = MotionPluginHelper.EnsureConnected(card, Model.AutoConnect);
                if (err != null) return Fail(err);
                err = MotionPluginHelper.EnsureEnabled(card, Model.AxisNo, Model.AutoEnable);
                if (err != null) return Fail(err);
                err = MotionPluginHelper.CheckMotionAllowed(card);
                if (err != null) return Fail(err);

                double target = GetValue(Model.TargetPosition);
                double vel = GetValue(Model.Velocity);
                res["目标位置"] = target;
                Log(3, $"绝对定位 卡={card.Name} 轴={Model.AxisNo} 目标={target:F3} 速度={vel:F3}");

                if (!card.MoveAbsolute(Model.AxisNo, target, vel))
                    return Fail("下发绝对定位失败");

                err = MotionPluginHelper.WaitIdle(card, Model.AxisNo, Model.WaitTimeoutMs, token);
                if (err != null) return Fail(err);

                var st = card.GetAxisStatus(Model.AxisNo);
                MotionPluginHelper.FillStatus(res, st);
                if (st == null) return Fail("无法读取轴状态");
                if (Math.Abs(st.Position - target) > Model.InPositionTolerance)
                    return Fail($"到位偏差超限: 实际 {st.Position:F3} 目标 {target:F3} 容差 {Model.InPositionTolerance}");

                res["结果"] = true;
                return NodeRunStatus.Success;
            }
            catch (OperationCanceledException) { throw; }
            catch (Exception ex) { return Fail(ex.Message); }
        }
    }

    [Serializable]
    [Plugin("相对定位", PluginCategory.硬件模块, typeof(MotionMoveRelModel), "ArrowExpandHorizontal",
        Description = "单轴相对定位，距离/速度可公式绑定，可勾选未使能时自动使能")]
    public class MotionMoveRelPlugin : BasePlugin<MotionMoveRelModel>
    {
        public override List<OutputField> DeclareOutputs() => new List<OutputField>
        {
            new OutputField("结果", typeof(bool)),
            new OutputField("最终位置", typeof(double)),
            new OutputField("相对距离", typeof(double)),
        };

        public override NodeRunStatus PluginRun(CancellationToken token, out Dictionary<string, object> results)
        {
            var res = new Dictionary<string, object>();
            results = res;
            NodeRunStatus Fail(string m)
            {
                res["Error"] = m;
                res["结果"] = false;
                Log(1, $"相对定位失败: {m}", TeamAAS.LogLevel.Error);
                return NodeRunStatus.Failed;
            }

            try
            {
                if (Model.AxisNo < 0) return Fail("定位必须指定具体轴号（不能为 -1）");

                var card = MotionPluginHelper.GetCard(Service<MotionManager>(), Model.MotionCardName, out var err);
                if (card == null) return Fail(err);
                err = MotionPluginHelper.EnsureConnected(card, Model.AutoConnect);
                if (err != null) return Fail(err);
                err = MotionPluginHelper.EnsureEnabled(card, Model.AxisNo, Model.AutoEnable);
                if (err != null) return Fail(err);
                err = MotionPluginHelper.CheckMotionAllowed(card);
                if (err != null) return Fail(err);

                double dist = GetValue(Model.Distance);
                double vel = GetValue(Model.Velocity);
                var before = card.GetAxisStatus(Model.AxisNo);
                double expect = (before?.Position ?? 0) + dist;
                res["相对距离"] = dist;
                Log(3, $"相对定位 卡={card.Name} 轴={Model.AxisNo} 距离={dist:F3} 速度={vel:F3}");

                if (!card.MoveRelative(Model.AxisNo, dist, vel))
                    return Fail("下发相对定位失败");

                err = MotionPluginHelper.WaitIdle(card, Model.AxisNo, Model.WaitTimeoutMs, token);
                if (err != null) return Fail(err);

                var st = card.GetAxisStatus(Model.AxisNo);
                MotionPluginHelper.FillStatus(res, st);
                if (st == null) return Fail("无法读取轴状态");
                if (Math.Abs(st.Position - expect) > Model.InPositionTolerance)
                    return Fail($"到位偏差超限: 实际 {st.Position:F3} 期望 {expect:F3} 容差 {Model.InPositionTolerance}");

                res["结果"] = true;
                return NodeRunStatus.Success;
            }
            catch (OperationCanceledException) { throw; }
            catch (Exception ex) { return Fail(ex.Message); }
        }
    }

    [Serializable]
    [Plugin("轴停止", PluginCategory.硬件模块, typeof(MotionStopModel), "StopCircle",
        Description = "平滑停止指定轴，或对整卡急停")]
    public class MotionStopPlugin : BasePlugin<MotionStopModel>
    {
        public override List<OutputField> DeclareOutputs() => new List<OutputField>
        {
            new OutputField("结果", typeof(bool)),
            new OutputField("最终位置", typeof(double)),
        };

        public override NodeRunStatus PluginRun(CancellationToken token, out Dictionary<string, object> results)
        {
            var res = new Dictionary<string, object>();
            results = res;
            NodeRunStatus Fail(string m)
            {
                res["Error"] = m;
                res["结果"] = false;
                Log(1, $"轴停止失败: {m}", TeamAAS.LogLevel.Error);
                return NodeRunStatus.Failed;
            }

            try
            {
                var card = MotionPluginHelper.GetCard(Service<MotionManager>(), Model.MotionCardName, out var err);
                if (card == null) return Fail(err);
                err = MotionPluginHelper.EnsureConnected(card, Model.AutoConnect);
                if (err != null) return Fail(err);

                Log(3, $"轴停止 卡={card.Name} 轴={Model.AxisNo} 急停={Model.Emergency}");
                bool ok = Model.Emergency ? card.EmergencyStop() : card.Stop(Model.AxisNo);
                if (!ok) return Fail("下发停止失败");

                if (Model.AxisNo >= 0)
                    MotionPluginHelper.FillStatus(res, card.GetAxisStatus(Model.AxisNo));

                res["结果"] = true;
                return NodeRunStatus.Success;
            }
            catch (OperationCanceledException) { throw; }
            catch (Exception ex) { return Fail(ex.Message); }
        }
    }
}