XplorePlane/XP.Hardware.MotionControl/Implementations/SimulatedRotaryAxis.cs

using System;
using System.Threading;
using System.Threading.Tasks;
using XP.Hardware.MotionControl.Abstractions;
using XP.Hardware.MotionControl.Abstractions.Enums;

namespace XP.Hardware.MotionControl.Implementations
{
    /// <summary>
    /// 虚拟旋转轴实现 | Simulated Rotary Axis Implementation
    /// 通过后台任务线性插值模拟旋转轴移动，无需真实 PLC 硬件
    /// Simulates rotary axis movement via background task with linear interpolation, no real PLC required
    /// </summary>
    public class SimulatedRotaryAxis : RotaryAxisBase
    {
        private const int UpdateIntervalMs = 20;

        private readonly double _defaultSpeed;
        private readonly object _lock = new object();

        private CancellationTokenSource _moveCts;
        private Task _moveTask;
        private double _jogSpeedPercent = 50.0;

        /// <summary>
        /// 构造函数 | Constructor
        /// </summary>
        /// <param name="axisId">轴标识 | Axis identifier</param>
        /// <param name="minAngle">最小角度（度）| Minimum angle (degrees)</param>
        /// <param name="maxAngle">最大角度（度）| Maximum angle (degrees)</param>
        /// <param name="enabled">是否启用 | Is enabled</param>
        /// <param name="defaultSpeed">默认角速度（度/秒）| Default angular speed (degrees/s)</param>
        public SimulatedRotaryAxis(RotaryAxisId axisId, double minAngle, double maxAngle, bool enabled, double defaultSpeed = 30.0)
            : base(axisId, minAngle, maxAngle, enabled)
        {
            _defaultSpeed = defaultSpeed;
        }

        /// <inheritdoc/>
        public override MotionResult MoveToTarget(double targetAngle, double? speed = null)
        {
            if (!ValidateTarget(targetAngle))
                return MotionResult.Fail($"[Simulated] 目标角度 {targetAngle} 超出范围 [{_minAngle}, {_maxAngle}]");

            lock (_lock)
            {
                if (Status == AxisStatus.Moving)
                {
                    CancelCurrentMove();
                }

                var moveSpeed = speed ?? _defaultSpeed;
                Status = AxisStatus.Moving;

                _moveCts = new CancellationTokenSource();
                var token = _moveCts.Token;

                _moveTask = Task.Run(() => ExecuteMove(targetAngle, moveSpeed, token), token);
            }

            return MotionResult.Ok();
        }

        /// <inheritdoc/>
        public override MotionResult Stop()
        {
            lock (_lock)
            {
                CancelCurrentMove();
                Status = AxisStatus.Idle;
            }

            return MotionResult.Ok();
        }

        /// <inheritdoc/>
        public override MotionResult Home()
        {
            lock (_lock)
            {
                if (Status == AxisStatus.Moving)
                {
                    CancelCurrentMove();
                }

                Status = AxisStatus.Homing;

                _moveCts = new CancellationTokenSource();
                var token = _moveCts.Token;

                _moveTask = Task.Run(async () =>
                {
                    await ExecuteMove(0, _defaultSpeed, token);
                    if (!token.IsCancellationRequested)
                    {
                        Status = AxisStatus.Idle;
                    }
                }, token);
            }

            return MotionResult.Ok();
        }

        /// <inheritdoc/>
        public override MotionResult JogStart(bool positive)
        {
            lock (_lock)
            {
                if (Status == AxisStatus.Moving || Status == AxisStatus.Homing)
                {
                    CancelCurrentMove();
                }

                var target = positive ? _maxAngle : _minAngle;
                var jogSpeed = _defaultSpeed * (_jogSpeedPercent / 100.0);

                Status = AxisStatus.Moving;

                _moveCts = new CancellationTokenSource();
                var token = _moveCts.Token;

                _moveTask = Task.Run(() => ExecuteMove(target, jogSpeed, token), token);
            }

            return MotionResult.Ok();
        }

        /// <inheritdoc/>
        public override MotionResult JogStop()
        {
            lock (_lock)
            {
                CancelCurrentMove();
                Status = AxisStatus.Idle;
            }

            return MotionResult.Ok();
        }

        /// <inheritdoc/>
        public override MotionResult SetJogSpeed(double speedPercent)
        {
            if (speedPercent < 0 || speedPercent > 100)
                return MotionResult.Fail($"[Simulated] Jog 速度百分比 {speedPercent} 超出范围 [0, 100]");

            _jogSpeedPercent = speedPercent;
            return MotionResult.Ok();
        }

        /// <inheritdoc/>
        public override void UpdateStatus()
        {
            // 虚拟轴无需从 PLC 轮询状态 | No PLC polling needed for simulated axis
        }

        /// <summary>
        /// 执行线性插值旋转移动 | Execute linear interpolation rotary move
        /// </summary>
        private async Task ExecuteMove(double targetAngle, double speed, CancellationToken token)
        {
            var startAngle = ActualAngle;
            var distance = targetAngle - startAngle;

            if (Math.Abs(distance) < 0.001)
            {
                Status = AxisStatus.Idle;
                return;
            }

            var totalTime = Math.Abs(distance) / speed; // seconds
            var elapsed = 0.0;

            while (!token.IsCancellationRequested)
            {
                try
                {
                    await Task.Delay(UpdateIntervalMs, token);
                }
                catch (TaskCanceledException)
                {
                    return;
                }

                elapsed += UpdateIntervalMs / 1000.0;

                if (elapsed >= totalTime)
                {
                    ActualAngle = targetAngle;
                    Status = AxisStatus.Idle;
                    return;
                }

                var progress = elapsed / totalTime;
                ActualAngle = startAngle + distance * progress;
            }
        }

        /// <summary>
        /// 取消当前移动任务 | Cancel current move task
        /// </summary>
        private void CancelCurrentMove()
        {
            if (_moveCts != null)
            {
                _moveCts.Cancel();
                _moveCts.Dispose();
                _moveCts = null;
            }
        }
    }
}