ParkingRobot/MultiWheelC/Movements/PrepareWheelsForward.cs

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

namespace MultiWheelC
{
    // C层测试准备：停车并等待四个舵轮稳定回到车体前向0°。
    public class PrepareWheelsForward : MovementDefinition
    {
        public float ToleranceDegrees = 2f;
        public float StableSeconds = 0.3f;
        public float TimeoutSeconds = 10f;
        public bool Completed { get; private set; }

        public override IEnumerable<bool> Get()
        {
            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();
            }
        }
    }
}