拆分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,123 @@
+using System;
+using System.Collections.Generic;
+using System.Threading;
+using ClumsyCore.Pilot;
+
+namespace MultiWheelC
+{
+    public class Sleep : MovementDefinition
+    {
+        public float Second = 2f;
+
+        public override IEnumerable<bool> Get()
+        {
+            if (Second <= 0)
+            {
+                yield return false;
+                yield break;
+            }
+
+            var endTime = DateTime.UtcNow.AddSeconds(Second);
+            while (DateTime.UtcNow < endTime)
+            {
+                Thread.Sleep(50);
+                yield return true;
+            }
+
+            yield return false;
+        }
+    }
+
+    public class DriverAble : MovementDefinition
+    {
+        public int WaitTimeoutMs = 2000;
+        public int PollIntervalMs = 50;
+
+        // C层单车硬件：请求全部驱动轮复位并恢复使能。
+        public override IEnumerable<bool> Get()
+        {
+            PilotDefinition.Self.ResetFromC = true;
+
+            try
+            {
+                var start = DateTime.Now;
+                var timeoutMs = Math.Max(0, WaitTimeoutMs);
+                var pollMs = Math.Max(1, PollIntervalMs);
+
+                // 至少保留一个调度周期，确保M层能收到复位请求。
+                yield return true;
+
+                while (!PilotDefinition.Self.WheelAbleState &&
+                        (DateTime.Now - start).TotalMilliseconds < timeoutMs)
+                {
+                    Thread.Sleep(pollMs);
+                    yield return true;
+                }
+            }
+            finally
+            {
+                PilotDefinition.Self.ResetFromC = false;
+            }
+        }
+    }
+    public class DriverDisable : MovementDefinition
+    {
+        public int WaitTimeoutMs = 3000;
+        public int PollIntervalMs = 20;
+
+        // C层单车硬件：请求驱动轮退出使能，并等待M层状态反馈。
+        public override IEnumerable<bool> Get()
+        {
+            var timeoutMs = Math.Max(0, WaitTimeoutMs);
+            var pollMs = Math.Max(1, PollIntervalMs);
+            var startTime = DateTime.UtcNow;
+            var success = false;
+
+            PilotDefinition.Self.DisableFromC = true;
+
+            try
+            {
+                // 至少保持一个C层调度周期，确保M层能收到下使能请求。
+                yield return true;
+
+                success = !PilotDefinition.Self.WheelAbleState;
+
+                while (!success &&
+                        (DateTime.UtcNow - startTime).TotalMilliseconds <
+                            timeoutMs)
+                {
+                    Thread.Sleep(pollMs);
+
+                    success =
+                        !PilotDefinition.Self.WheelAbleState;
+
+                    if (!success)
+                    {
+                        yield return true;
+                    }
+                }
+            }
+            finally
+            {
+                // 无论正常完成、超时、异常还是任务被停止，都撤销请求。
+                PilotDefinition.Self.DisableFromC = false;
+            }
+            if (success)
+            {
+                Console.WriteLine(
+                    $"驱动器下使能完成，" +
+                    $"WheelAbleState=" +
+                    $"{PilotDefinition.Self.WheelAbleState}");
+            }
+            else
+            {
+                Console.WriteLine(
+                    $"驱动器下使能超时，" +
+                    $"WheelAbleState=" +
+                    $"{PilotDefinition.Self.WheelAbleState}，" +
+                    $"等待{timeoutMs}ms");
+            }
+            yield return false;
+        }
+    }
+}