ParkingRobot/MultiWheelC/Movements/PrepareWheelsForward.cs

using System;
using System.Collections.Generic;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using MyParking.Shared;

namespace MultiWheelC
{
    /// <summary>
    /// 停车并等待四个舵轮稳定回到车体前向0°。
    /// </summary>
    public class PrepareWheelsForward : MovementDefinition
    {
        /// <summary>
        /// 获取或设置本次动作的回正到位容差覆盖值，单位为deg；为空时读取车辆配置。
        /// </summary>
        public float? ToleranceDegrees;

        /// <summary>
        /// 获取或设置本次动作的稳定确认时间覆盖值，单位为s；为空时读取车辆配置。
        /// </summary>
        public float? StableSeconds;

        /// <summary>
        /// 获取或设置本次动作的超时覆盖值，单位为s；为空时读取车辆配置，0表示关闭超时。
        /// </summary>
        public float? TimeoutSeconds;

        public bool Completed { get; private set; }

        /// <summary>
        /// 读取一次有效配置并等待全部舵轮在容差内稳定保持车体前向0°。
        /// </summary>
        public override IEnumerable<bool> Get()
        {
            var config = PilotDefinition.Conf;
            var toleranceDegrees =
                ToleranceDegrees ??
                config.ParkingWheelForwardToleranceDegrees;
            var stableSeconds =
                StableSeconds ??
                config.ParkingWheelForwardStableSeconds;
            var timeoutSeconds =
                TimeoutSeconds ??
                config.ParkingWheelForwardTimeoutSeconds;

            NumericGuard.EnsureFiniteNonNegative(
                toleranceDegrees,
                nameof(ToleranceDegrees));
            NumericGuard.EnsureFiniteNonNegative(
                stableSeconds,
                nameof(StableSeconds));
            NumericGuard.EnsureFiniteNonNegative(
                timeoutSeconds,
                nameof(TimeoutSeconds));

            var chassis =
                PilotDefinition.Chassis as MultiWheelChassis;
            if (chassis == null)
            {
                throw new InvalidOperationException(
                    "当前底盘不是MultiWheelChassis，无法执行舵轮回正。");
            }

            var adapter = new MultiWheelChassisAdapter(
                chassis,
                PilotDefinition.Self.CarNum);
            adapter.ResetToBodyFrame();
            var toleranceRadians =
                AngleMath.DegreesToRadians(toleranceDegrees);
            var startTime = DateTime.UtcNow;
            DateTime? alignedSince = null;

            Completed = false;
            if (!adapter.PrepareParallelDirection(0.0))
            {
                throw new InvalidOperationException(
                    "无法将所有舵轮下发到车体前向0°。");
            }

            try
            {
                while (true)
                {
                    var aligned =
                        adapter.AreParallelWheelsAligned(
                            0.0,
                            toleranceRadians);

                    if (aligned)
                    {
                        if (!alignedSince.HasValue)
                            alignedSince = DateTime.UtcNow;

                        if ((DateTime.UtcNow -
                                alignedSince.Value).TotalSeconds >=
                            stableSeconds)
                        {
                            Completed = true;
                            yield break;
                        }
                    }
                    else
                    {
                        alignedSince = null;
                    }

                    if (timeoutSeconds > 0f &&
                        (DateTime.UtcNow - startTime).TotalSeconds >
                        timeoutSeconds)
                    {
                        throw new TimeoutException(
                            $"舵轮回正超过{timeoutSeconds:F1}s，" +
                            "测试已经取消。");
                    }

                    yield return true;
                }
            }
            finally
            {
                // 只清零驱动速度，保留已经下发的0°舵角。
                adapter.StopImmediately();
            }
        }
    }
}