拆分MultiWheelC并新增轨迹投影、Detour状态估计与Stanley跟踪控制

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-08-05 18:07:01 +08:00
parent 7447317812
commit 47973cc94b
38 changed files with 3982 additions and 976 deletions
@@ -0,0 +1,158 @@
+
+
+using System;
+using System.Numerics;
+using System.Threading;
+using ClumsyCore;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+using MDCSToolBox.Commons.Controllers;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    public abstract class InPlaceRotateTestBase : MovementTest
+    {
+        public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。
+        public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。
+        public int TrialNumber = 1; // 重复实验编号。
+
+        private DriveTask _task;
+        private TrackingExperimentRecorder _recorder;
+        private readonly string _trajectoryName;
+
+        protected InPlaceRotateTestBase(
+            float relativeAngleDegrees,
+            string trajectoryName)
+        {
+            RelativeAngleDegrees =
+                relativeAngleDegrees;
+            _trajectoryName =
+                trajectoryName;
+        }
+
+        // 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。
+        public override void Test()
+        {
+            if (float.IsNaN(RelativeAngleDegrees) ||
+                float.IsInfinity(RelativeAngleDegrees) ||
+                float.IsNaN(MaxAngularSpeedDegreesPerSecond) ||
+                float.IsInfinity(MaxAngularSpeedDegreesPerSecond) ||
+                MaxAngularSpeedDegreesPerSecond <= 0f)
+            {
+                Console.WriteLine("原地旋转测试参数无效。");
+                return;
+            }
+
+            var location = DetourInterface.getCartLocation();
+            if (double.IsNaN(location.x) ||
+                double.IsInfinity(location.x) ||
+                double.IsNaN(location.y) ||
+                double.IsInfinity(location.y) ||
+                double.IsNaN(location.th) ||
+                double.IsInfinity(location.th))
+            {
+                Console.WriteLine(
+                    "Detour当前位姿无效，取消原地旋转测试。");
+                return;
+            }
+
+            var rotationCenter =
+                new Vector2((float)location.x, (float)location.y);
+            var targetWorldAngle =
+                (float)AngleMath.NormalizeDegrees(
+                    location.th + RelativeAngleDegrees);
+
+            _recorder = new TrackingExperimentRecorder(
+                controllerName: "InPlaceRotatePID",
+                trajectoryName: _trajectoryName,
+                trialNumber: TrialNumber,
+                referenceStart: rotationCenter,
+                referenceEnd: rotationCenter,
+                referenceSpeed: 0f,
+                referenceAngularSpeed:
+                    (float)AngleMath.DegreesToRadians(
+                        MaxAngularSpeedDegreesPerSecond));
+            _recorder.Start();
+
+            try
+            {
+                _task = new DriveTask(
+                    new MultiWheelRotateInPlace
+                    {
+                        // MultiWheelRotateInPlace接收世界坐标系绝对航向。
+                        AngleTarget = targetWorldAngle,
+                        PidparamsRead = () => new PIDParams
+                        {
+                            Kp =
+                                PilotDefinition.Conf.InPlaceRotateKp,
+                            Ki =
+                                PilotDefinition.Conf.InPlaceRotateKi,
+                            Kd =
+                                PilotDefinition.Conf.InPlaceRotateKd,
+                            DeadZone =
+                                PilotDefinition.Conf
+                                    .InPlaceRotateArriveDeg,
+                            SpeedAccPerSec =
+                                PilotDefinition.Conf.InPlaceRotateAcc,
+                            OutputUpperThreshold =
+                                MaxAngularSpeedDegreesPerSecond,
+                            MaxI =
+                                PilotDefinition.Conf.InPlaceRotateMaxI
+                        },
+                        CommandAngularSpeedObserver =
+                            commandAngularSpeed =>
+                                _recorder?.UpdateCommand(
+                                    0f,
+                                    (float)AngleMath.DegreesToRadians(
+                                        commandAngularSpeed))
+                    }.Get());
+
+                _task.Wait();
+
+                // 保留少量停止后的样本，用于观察角速度是否回到零。
+                Thread.Sleep(300);
+            }
+            finally
+            {
+                _task?.Stop();
+                _recorder?.UpdateCommand(0f, 0f);
+                _recorder?.StopAndSave();
+                _task = null;
+                _recorder = null;
+            }
+        }
+
+        // 停止原地旋转并保存当前已经采集的实验数据。
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _recorder?.UpdateCommand(0f, 0f);
+            _recorder?.StopAndSave();
+        }
+
+    }
+
+    [MovementTest(name = "SendXYThSpeed：原地自转90°")]
+    public sealed class TestRotate90 :
+        InPlaceRotateTestBase
+    {
+        public TestRotate90()
+            : base(90f, "Rotate90")
+        {
+        }
+    }
+
+    [MovementTest(name = "SendXYThSpeed：原地自转180°")]
+    public sealed class TestRotate180 :
+        InPlaceRotateTestBase
+    {
+        public TestRotate180()
+            : base(180f, "Rotate180")
+        {
+        }
+    }
+}