From d3b4a9eb15759c2385e171cbabae76d84f5a5a41 Mon Sep 17 00:00:00 2001 From: "shuai.li" <1248399577@qq.com> Date: Thu, 2 Jul 2026 17:52:55 +0800 Subject: [PATCH] =?UTF-8?q?update=20=E5=A2=9E=E5=8A=A0=E8=BF=BD=E8=B8=AA?= =?UTF-8?q?=E7=82=B9=E5=8A=A8=E4=BD=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MultiWheel/MultiWheelC/AGV.cs | 15 ++- .../MovementTests.TireFollowing.cs | 125 +++++++++++++++++- MultiWheel/MultiWheelC/Movements.cs | 27 ++++ MultiWheel/MultiWheelC/PilotConfig.cs | 2 + 4 files changed, 165 insertions(+), 4 deletions(-) diff --git a/MultiWheel/MultiWheelC/AGV.cs b/MultiWheel/MultiWheelC/AGV.cs index 8e06b04..169899f 100644 --- a/MultiWheel/MultiWheelC/AGV.cs +++ b/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) diff --git a/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs b/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs index 23fa73a..a3fc5fb 100644 --- a/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs +++ b/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 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() diff --git a/MultiWheel/MultiWheelC/Movements.cs b/MultiWheel/MultiWheelC/Movements.cs index 21b7bad..8480ae7 100644 --- a/MultiWheel/MultiWheelC/Movements.cs +++ b/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 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 { diff --git a/MultiWheel/MultiWheelC/PilotConfig.cs b/MultiWheel/MultiWheelC/PilotConfig.cs index 7eb4116..71b58bf 100644 --- a/MultiWheel/MultiWheelC/PilotConfig.cs +++ b/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 }