update 增加追踪点动作

MultiWheel/MultiWheelC/AGV.cs

@@ -226,7 +226,7 @@ namespace MultiWheelC
                 RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos
             }.Get()).Wait();
         }
-        public void LineTracking(int srcId, int dstId)
+        public void LineTracking(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
         {
             while (!TryLock(dstId))
             {
@@ -236,9 +236,18 @@ namespace MultiWheelC
             new DriveTask(new LineTracking()
             {
                 Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
-                LeaveSrcFunction = Leave,
-                SrcId = srcId,
+                //LeaveSrcFunction = Leave,
+                //SrcId = srcId,
             }.Get()).Wait();
+
+            new DriveTask(new DstTracker()
+            {
+                Src = new Vector2(srcX, srcY),
+                Dst = new Vector2(dstX, dstY),
+            }.Get());Wait();
+            Leave(srcId);
+            DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
+
         }
 
         public void ChangeAvoidanceDistance(float stopDistance, float slowDistance)

MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs

@@ -292,7 +292,130 @@ namespace MultiWheelC
         private DriveTask _dt;
     }
 
-    [MovementTest(name = "驱动器下使能测试")]
+    [MovementTest(name = "测试终点跟踪动作-前进")]
+    public class DstTrackerForward : MovementTest
+    {
+        public bool UseInteractivePick = true;
+        public float srcX;
+        public float srcY;
+        public float dstX;
+        public float dstY;
+        public float carDirectionBias = 0f;
+        private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
+
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            _painter?.Clear();
+        }
+        public override void Test()
+        {
+            var p1 = UI.GetPoint("point1");
+            var p2 = UI.GetPoint("point2");
+
+            _painter.Clear();
+            _dt = new DriveTask(new DstTracker()
+            {
+                Src = p1,
+                Dst = p2,
+                CarDirectionBias = carDirectionBias,
+            }.Get());
+            _dt.Wait();
+        }
+
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "测试终点跟踪动作-后退")]
+    public class DstTrackerhoutui : MovementTest
+    {
+        public bool UseInteractivePick = true;
+        public float srcX;
+        public float srcY;
+        public float dstX;
+        public float dstY;
+        public float carDirectionBias = 180f;
+        private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
+
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            _painter?.Clear();
+        }
+        public override void Test()
+        {
+            var p1 = UI.GetPoint("point1");
+            var p2 = UI.GetPoint("point2");
+
+            _painter.Clear();
+            _dt = new DriveTask(new DstTracker()
+            {
+                Src = p1,
+                Dst = p2,
+                CarDirectionBias = carDirectionBias,
+            }.Get());
+            _dt.Wait();
+        }
+
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "测试先直行再终点跟踪")]
+    public class LineTrackThenDstTrackerTest : MovementTest
+    {
+        public float carDirectionBias = 0f;
+        private readonly Painter _painter = UI.GetPainter("LineTrackThenDstTrackerTest");
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            _painter?.Clear();
+        }
+        public override void Test()
+        {
+            var src = UI.GetPoint("请在上位机选择起点(src)");
+            var dst = UI.GetPoint("请在上位机选择终点(dst)");
+            _painter.Clear();
+            _painter.DrawLine(Color.Cyan, src.X, src.Y, dst.X, dst.Y, width: 3);
+            _painter.DrawCircle(Color.LimeGreen, src.X, src.Y, 80f);
+            _painter.DrawCircle(Color.OrangeRed, dst.X, dst.Y, 80f);
+            _painter.DrawText(Color.LimeGreen, "src", src.X + 80f, src.Y + 80f);
+            _painter.DrawText(Color.OrangeRed, "dst", dst.X + 80f, dst.Y + 80f);
+            IEnumerable<bool> TrackThenFollow()
+            {
+                foreach (var running in new LineTracking()
+                {
+                    Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                // 衔接速度：避免第一段结束后瞬间“掉速”，让第二段接管更连贯
+                var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+                var handoverSpeed = Math.Max(0.08f, Math.Min(PilotDefinition.Conf.DstTrackerMaxSpeed, 0.2f));
+                var direction = Math.Sign(dst.x - src.x);
+                if (direction == 0) direction = 1;
+                chassis.SendXYThSpeed(handoverSpeed * direction, 0f, 0f);
+                yield return true;
+                foreach (var running in new DstTracker()
+                {
+                    Src = src,
+                    Dst = dst,
+                    CarDirectionBias = carDirectionBias,
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                yield return false;
+            }
+            _dt = new DriveTask(TrackThenFollow());
+            _dt.Wait();
+        }
+        private DriveTask _dt;
+    }
+
+
     public class DriverDisableTest : MovementTest
     {
         public override void TestStop()

MultiWheel/MultiWheelC/Movements.cs

@@ -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
     {

MultiWheel/MultiWheelC/PilotConfig.cs

@@ -287,5 +287,7 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "轮胎跟踪:距离过近角度忽略阈值")] public float TireFollowingAngleIgnoreThr = 0.2f;
     [FieldMember(desc = "轮胎跟踪:Y最大平均数")] public int TireFollowingYAverageFrameCount = 5;
 
+    [FieldMember(desc = "终点跟踪:速度")] public float DstTrackerMaxSpeed = 0.3f;
+
     #endregion
 }