完善原地自转控制逻辑并加入纵向速度死区与实验绘图改进

Co-authored-by: Cursor <cursoragent@cursor.com>

MultiWheelC/Movements/RotateInPlace.cs

@@ -7,6 +7,7 @@ using ClumsyCore.Pilot;
 using CommonUsage.Chassis;
 using MDCSToolBox.Commons.Controllers;
 using MyParking.Shared;
+using MultiWheelC.StateEstimation;
 
 namespace MultiWheelC
 {
@@ -17,7 +18,11 @@ namespace MultiWheelC
         /// </summary>
         public float AngleTarget;
 
-        public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
+        // 留作标定或单元测试时显式替换；为空时使用经过校验的Detour状态源。
+        public Func<float> ThetaReader;
+
+        public IVehicleStateProvider StateProvider =
+            new DetourVehicleStateProvider();
 
         public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
 
@@ -37,6 +42,12 @@ namespace MultiWheelC
         // 自转舵轮准备超时时间，单位s。
         public float WheelAlignmentTimeoutSeconds = 10f;
 
+        // 航向尚未到位时允许下发的最小有效角速度，单位deg/s。
+        public float MinimumAngularSpeedDegreesPerSecond = 1f;
+
+        // 舵轮到位后执行航向闭环允许的最长时间，单位s。
+        public float RotationTimeoutSeconds = 15f;
+
         // 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。
         public override IEnumerable<bool> Get()
         {
@@ -44,6 +55,8 @@ namespace MultiWheelC
                 throw new InvalidOperationException(
                     "当前底盘不是MultiWheelChassis，无法执行原地自转。");
 
+            ValidateParameters();
+
             var adapter = new MultiWheelChassisAdapter(
                 Chassis,
                 PilotDefinition.Self.CarNum);
@@ -55,7 +68,9 @@ namespace MultiWheelC
                 DateTime? alignedSince = null;
                 while (true)
                 {
-                    if (!adapter.PrepareSpin())
+                    if (!adapter.PrepareSpin(
+                            alignmentToleranceDegrees:
+                                WheelAlignmentToleranceDegrees))
                         throw new InvalidOperationException(
                             "无法生成原地自转舵轮目标：" +
                             adapter.LastFailureReason);
@@ -84,18 +99,86 @@ namespace MultiWheelC
                     yield return true;
                 }
 
+                var alignmentToleranceRadians =
+                    AngleMath.DegreesToRadians(
+                        WheelAlignmentToleranceDegrees);
+                if (!adapter.AdoptPreparedSpinForXYTh(
+                        alignmentToleranceRadians))
+                {
+                    throw new InvalidOperationException(
+                        "无法将已到位的自转舵角交接给XYTh：" +
+                        adapter.LastFailureReason);
+                }
+
                 var targetAngle =
                     (float)AngleMath.NormalizeDegrees(AngleTarget);
                 var p = PidparamsRead();
-                thPid = new PIDController(ThetaReader, p.Kp);
+                var currentAngle = ReadCurrentAngleDegrees();
+                var cachedCurrentAngle = currentAngle;
+                thPid = new PIDController(
+                    () => cachedCurrentAngle,
+                    p.Kp);
                 thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
                     p.OutputUpperThreshold, p.SpeedAccPerSec);
                 var lastCommandTime = DateTime.Now;
+                var rotationStarted = DateTime.Now;
 
                 while (true)
                 {
+                    if ((DateTime.Now - rotationStarted)
+                            .TotalSeconds >
+                        RotationTimeoutSeconds)
+                    {
+                        throw new TimeoutException(
+                            $"原地自转超过{RotationTimeoutSeconds:F1}s仍未到位。");
+                    }
+
+                    currentAngle = ReadCurrentAngleDegrees();
+                    cachedCurrentAngle = currentAngle;
                     var s = thPid.GetResponse(targetAngle, true);
-                    Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
+                    var angleErrorDegrees =
+                        (float)AngleMath
+                            .ShortestDifferenceDegrees(
+                                targetAngle,
+                                currentAngle);
+
+                    // PID进入到位死区后等待其0.3s稳定确认；等待期间
+                    // 只清零驱动速度，不清除已经准备好的自转舵角状态。
+                    if (Math.Abs(angleErrorDegrees) <=
+                        p.DeadZone)
+                    {
+                        CommandAngularSpeedObserver?.Invoke(0f);
+                        adapter
+                            .StopXYThDrivePreserveSteeringState();
+
+                        if (thPid.IsArrived())
+                            break;
+
+                        yield return true;
+                        continue;
+                    }
+
+                    // PID输出低于底盘有效轮速范围时提高到最小可执行值，
+                    // 避免接近目标时反复出现微小命令但车辆实际不动。
+                    if (Math.Abs(s) > 1e-6f &&
+                        Math.Abs(s) <
+                            MinimumAngularSpeedDegreesPerSecond)
+                    {
+                        s = Math.Sign(angleErrorDegrees) *
+                            MinimumAngularSpeedDegreesPerSecond;
+                    }
+
+                    // PID加速限制在首周期可能暂时输出零；此时保留
+                    // 已交接的自转状态，等待下一周期产生有效角速度。
+                    if (Math.Abs(s) <= 1e-6f)
+                    {
+                        CommandAngularSpeedObserver?.Invoke(0f);
+                        adapter
+                            .StopXYThDrivePreserveSteeringState();
+                        yield return true;
+                        continue;
+                    }
+
                     CommandAngularSpeedObserver?.Invoke(s);
                     var now = DateTime.Now;
                     var interval = now - lastCommandTime;
@@ -118,7 +201,6 @@ namespace MultiWheelC
                             "安全XYTh原地旋转底盘解算失败：" +
                             adapter.LastFailureReason);
                     }
-                    if (thPid.IsArrived()) break;
                     yield return true;
                 }
 
@@ -130,5 +212,109 @@ namespace MultiWheelC
                 adapter.StopImmediately();
             }
         }
+
+        /// <summary>
+        /// 检查原地自转的舵轮准备、最小速度和超时参数是否可执行。
+        /// </summary>
+        private void ValidateParameters()
+        {
+            EnsureFinitePositive(
+                WheelAlignmentToleranceDegrees,
+                nameof(WheelAlignmentToleranceDegrees),
+                allowZero: true);
+            EnsureFinitePositive(
+                WheelAlignmentStableSeconds,
+                nameof(WheelAlignmentStableSeconds),
+                allowZero: true);
+            EnsureFinitePositive(
+                WheelAlignmentTimeoutSeconds,
+                nameof(WheelAlignmentTimeoutSeconds));
+            EnsureFinitePositive(
+                MinimumAngularSpeedDegreesPerSecond,
+                nameof(MinimumAngularSpeedDegreesPerSecond));
+            EnsureFinitePositive(
+                RotationTimeoutSeconds,
+                nameof(RotationTimeoutSeconds));
+
+            var pidParameters = PidparamsRead();
+            if (pidParameters == null)
+            {
+                throw new InvalidOperationException(
+                    "原地自转PID参数读取结果为空。");
+            }
+
+            EnsureFinitePositive(
+                pidParameters.DeadZone,
+                "PidparamsRead.DeadZone");
+            EnsureFinitePositive(
+                pidParameters.OutputUpperThreshold,
+                "PidparamsRead.OutputUpperThreshold");
+            EnsureFinitePositive(
+                pidParameters.SpeedAccPerSec,
+                "PidparamsRead.SpeedAccPerSec");
+            EnsureFinitePositive(
+                pidParameters.Kp,
+                "PidparamsRead.Kp");
+
+            if (MinimumAngularSpeedDegreesPerSecond >
+                pidParameters.OutputUpperThreshold)
+            {
+                throw new InvalidOperationException(
+                    "原地自转最小有效角速度不能大于最大角速度。");
+            }
+        }
+
+        /// <summary>
+        /// 读取经过状态源校验的世界航向，显式设置ThetaReader时优先使用替代读数。
+        /// </summary>
+        private float ReadCurrentAngleDegrees()
+        {
+            if (ThetaReader != null)
+            {
+                var angleDegrees = ThetaReader();
+                if (float.IsNaN(angleDegrees) ||
+                    float.IsInfinity(angleDegrees))
+                {
+                    throw new InvalidOperationException(
+                        "自定义航向读取结果不是有效角度。");
+                }
+
+                return (float)AngleMath.NormalizeDegrees(
+                    angleDegrees);
+            }
+
+            if (StateProvider == null ||
+                !StateProvider.TryGetState(out var state))
+            {
+                throw new InvalidOperationException(
+                    "无法从Detour状态源读取有效车辆航向。" +
+                    (StateProvider is DetourVehicleStateProvider provider
+                        ? provider.LastFailureReason
+                        : ""));
+            }
+
+            return (float)AngleMath.RadiansToDegrees(
+                state.PoseInWorld.YawRadians);
+        }
+
+        /// <summary>
+        /// 检查原地自转参数是否为正有限值，部分时间和容差参数允许为零。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            float value,
+            string parameterName,
+            bool allowZero = false)
+        {
+            if (float.IsNaN(value) ||
+                float.IsInfinity(value) ||
+                (allowZero
+                    ? value < 0f
+                    : value <= 0f))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "原地自转参数必须是有效的正数。");
+            }
+        }
     }
 }