update 增加追踪点动作

2026-07-02 17:52:55 +08:00
parent 71444b362d
commit d3b4a9eb15
4 changed files with 165 additions and 4 deletions
@@ -167,6 +167,33 @@ namespace MultiWheelC
        }
    }

+    //在世界坐标系下，从路径起点追踪到终点并停车
+    public class DstTracker : MovementDefinition
+    {
+        public Vector2 Src;
+        public Vector2 Dst;
+        public float CarDirectionBias = 0f;
+        public Painter Painter = UI.GetPainter("DstTracker");
+
+        public override IEnumerable<bool> Get()
+        {
+            Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
+            Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
+
+            var tracker = new ChassisController().Get();
+            var linePath = new LineTrack(Src, Dst) { CarDirectionBias = CarDirectionBias, Speed = PilotDefinition.Conf.DstTrackerMaxSpeed };
+            tracker.AddTrack(linePath);
+
+            var task = new DriveTask(tracker.Track());
+            task.Wait();
+
+            // 到点后兜底停车
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            chassis.SendXYThSpeed(0f, 0f, 0f);
+            yield return false;
+        }
+    }
+
    //直线行走基于轮里程
    public class LineTracking : MovementDefinition
    {