TeamAAS-2.5/TeamAAS.Core/TeamAAS.Motion/Motions/MotionSafetyGuard.cs

using System;
using System.Threading;
using TeamAAS.Global.Devices;
using TeamAAS.Motion.Models;

namespace TeamAAS.Motion.Motions
{
    /// <summary>
    /// 连接后轮询急停/停止 DI。急停边沿急停+持续下使能；停止边沿停轴并禁止新运动。
    /// </summary>
    internal sealed class MotionSafetyGuard : IDisposable
    {
        private readonly Func<MotionDeviceConfig> _cfg;
        private readonly Func<MotionIoBank, int, bool> _readDi;
        private readonly Action _onEstopRise;
        private readonly Action _onEstopHold;
        private readonly Action _onStopRise;
        private readonly object _sync = new object();
        private Timer _timer;
        private bool _estop;
        private bool _stop;

        public bool EstopActive { get { lock (_sync) return _estop; } }
        public bool StopActive { get { lock (_sync) return _stop; } }

        public event EventHandler Changed;

        public MotionSafetyGuard(
            Func<MotionDeviceConfig> cfg,
            Func<MotionIoBank, int, bool> readDi,
            Action onEstopRise,
            Action onEstopHold,
            Action onStopRise)
        {
            _cfg = cfg;
            _readDi = readDi;
            _onEstopRise = onEstopRise;
            _onEstopHold = onEstopHold;
            _onStopRise = onStopRise;
        }

        public void Start()
        {
            Stop();
            _timer = new Timer(_ => Poll(), null, 30, 30);
        }

        public void Stop()
        {
            var t = _timer;
            _timer = null;
            t?.Dispose();
            lock (_sync)
            {
                _estop = false;
                _stop = false;
            }
        }

        public bool AllowEnable(out string reason)
        {
            if (EstopActive)
            {
                reason = "急停有效：禁止使能";
                return false;
            }
            reason = null;
            return true;
        }

        public bool AllowMotion(out string reason)
        {
            if (EstopActive)
            {
                reason = "急停有效：禁止运动";
                return false;
            }
            if (StopActive)
            {
                reason = "停止信号有效：禁止运动";
                return false;
            }
            reason = null;
            return true;
        }

        public void Dispose() => Stop();

        private void Poll()
        {
            MotionDeviceConfig cfg;
            try { cfg = _cfg(); }
            catch { return; }
            if (cfg == null) return;

            bool estop = ReadActive(cfg.EstopDiBank, cfg.EstopDiIndex, cfg.EstopActiveHigh);
            bool stop = ReadActive(cfg.StopDiBank, cfg.StopDiIndex, cfg.StopActiveHigh);

            bool estopRise, stopRise, changed;
            lock (_sync)
            {
                estopRise = estop && !_estop;
                stopRise = stop && !_stop;
                changed = estop != _estop || stop != _stop;
                _estop = estop;
                _stop = stop;
            }

            try
            {
                if (estopRise)
                    _onEstopRise?.Invoke();
                if (estop)
                    _onEstopHold?.Invoke();
                else if (stopRise)
                    _onStopRise?.Invoke();
            }
            catch { }

            if (changed)
            {
                try { Changed?.Invoke(this, EventArgs.Empty); }
                catch { }
            }
        }

        private bool ReadActive(MotionIoBank bank, int di, bool activeHigh)
        {
            if (di < 0) return false;
            bool bit;
            try { bit = _readDi(bank, di); }
            catch { return false; }
            return activeHigh ? bit : !bit;
        }
    }
}