拆分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,90 @@
+
+
+using System;
+using ClumsyCore;
+using ClumsyCore.Pilot;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+
+namespace MultiWheelC
+{
+    public abstract class ClampMovementTestBase : MovementTest
+    {
+        public float TimeoutSeconds = 30f; // 动作超时时间，单位s。
+
+        private DriveTask _task;
+        protected abstract bool Close { get; }
+
+        // 根据派生测试类型驱动左右夹臂同步夹紧或打开。
+        public override void Test()
+        {
+            var leftTarget = Close
+                ? PilotDefinition.Self.LeftArmUpperPos
+                : PilotDefinition.Self.LeftArmLowerPos;
+            var rightTarget = Close
+                ? PilotDefinition.Self.RightArmUpperPos
+                : PilotDefinition.Self.RightArmLowerPos;
+
+            if (float.IsNaN(leftTarget) ||
+                float.IsInfinity(leftTarget) ||
+                float.IsNaN(rightTarget) ||
+                float.IsInfinity(rightTarget))
+            {
+                Console.WriteLine(
+                    "夹臂目标位置无效，取消夹臂运动测试。");
+                return;
+            }
+
+            // 防止重复点击时上一项夹臂任务仍在运行。
+            TestStop();
+            Console.WriteLine(
+                $"开始夹臂{(Close ? "夹紧" : "打开")}测试：" +
+                $"左目标={leftTarget}，右目标={rightTarget}");
+
+            var task = new DriveTask(
+                new ClampToTarget
+                {
+                    LeftClampTarget = leftTarget,
+                    RightClampTarget = rightTarget,
+                    TimeoutSeconds = TimeoutSeconds
+                }.Get());
+            _task = task;
+
+            try
+            {
+                task.Wait();
+            }
+            finally
+            {
+                PilotDefinition.Self.SpeedLeftArm = 0f;
+                PilotDefinition.Self.SpeedRightArm = 0f;
+                if (ReferenceEquals(_task, task))
+                    _task = null;
+            }
+        }
+
+        // 停止夹臂任务并立即清零左右夹臂下发速度。
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _task = null;
+            PilotDefinition.Self.SpeedLeftArm = 0f;
+            PilotDefinition.Self.SpeedRightArm = 0f;
+        }
+    }
+
+    [MovementTest(name = "夹臂关闭测试")]
+    public sealed class TestClampCloseMovement
+        : ClampMovementTestBase
+    {
+        protected override bool Close => false;
+    }
+
+    [MovementTest(name = "夹臂启动测试")]
+    public sealed class TestClampOpenMovement
+        : ClampMovementTestBase
+    {
+        protected override bool Close => true;
+    }
+}