From 6ca401cf68869a1650c1ef1283ebeb5200ccc250 Mon Sep 17 00:00:00 2001 From: "shuai.li" <1248399577@qq.com> Date: Tue, 7 Jul 2026 20:00:57 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E5=8E=9F=E5=9C=B0=E6=97=8B?= =?UTF-8?q?=E8=BD=AC=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 | 30 ++++++++++++++++++- .../MovementTests.TireFollowing.cs | 29 ++++++++++++++++++ MultiWheel/MultiWheelC/PilotConfig.cs | 10 ++++++- MultiWheel/MultiWheelC/TireFollowing.cs | 21 +++++++++---- 4 files changed, 82 insertions(+), 8 deletions(-) diff --git a/MultiWheel/MultiWheelC/AGV.cs b/MultiWheel/MultiWheelC/AGV.cs index 21de3aa..cf35434 100644 --- a/MultiWheel/MultiWheelC/AGV.cs +++ b/MultiWheel/MultiWheelC/AGV.cs @@ -69,6 +69,25 @@ namespace MultiWheelC PilotDefinition.Self.IOObstacleArea = area; } } + public void RotateToTarget(float target) + { + //if (!needrotate) return; + var dl = new DriveTask(new MultiWheelRotateInPlace() + { + AngleTarget = target, + PidparamsRead = () => new PIDParams() + { + Kp = PilotDefinition.Conf.TireFollowingThkp, + Ki = PilotDefinition.Conf.TireFollowingThki, + Kd = PilotDefinition.Conf.TireFollowingThkd, + DeadZone = PilotDefinition.Conf.TireFollowingThDeadZone, + SpeedAccPerSec = PilotDefinition.Conf.TireFollowingThSpeedAccPerSec, + OutputUpperThreshold = PilotDefinition.Conf.TireFollowingThThresh, + MaxI = PilotDefinition.Conf.TireFollowingThMaxI, + } + }.Get()); + dl.Wait(); + } //参数1：tireNum 需要钻过的轮胎对数量 //参数2：frontLidarDetect true:前雷达识别 false:后雷达识别 @@ -192,7 +211,7 @@ namespace MultiWheelC }, CarDirection = 180f, SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance, - MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed, + MaxSpeed = 0.25f, EnableHandover = true, HandoverDistance = 200f, HandoverSpeed = 0.3f, @@ -246,6 +265,9 @@ namespace MultiWheelC Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2, LeaveSrcFunction = Leave, SrcId = srcId, + EnableHandover = true, + HandoverDistance = 200, + HandoverSpeed = 0.3f, }.Get()) { if (!running) break; @@ -253,6 +275,12 @@ namespace MultiWheelC } DLog.Log($"释放锁点{srcId}完成", "TireFollowing"); DLog.Log("离车LineTracking结束，开始DstTracker", "TireFollowing"); + while (!TryLock(426)) + { + Thread.Sleep(20); + } + Leave(dstId); + DLog.Log($"释放锁点{dstId}完成", "TireFollowing"); foreach (var running in new DstTracker() { Src = new Vector2(srcX, srcY), diff --git a/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs b/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs index 7c60848..ff3598e 100644 --- a/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs +++ b/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs @@ -520,6 +520,35 @@ namespace MultiWheelC } } + [MovementTest(name = "搴曠洏鏃嬭浆娴嬭瘯")] + public class RotateToAngleTest : MovementTest + { + public override void TestStop() + { + throw new NotImplementedException(); + } + + public override void Test() + { + var target = UI.GetInput("杈撳叆鏃嬭浆瑙掑害锛"); + var chassis = (MultiWheelChassis)PilotDefinition.Chassis; + new DriveTask(new MultiWheelRotateInPlace() + { + AngleTarget = float.Parse(target), + PidparamsRead = () => new PIDParams() + { + Kp = PilotDefinition.Conf.TireFollowingThkp, + Ki = PilotDefinition.Conf.TireFollowingThki, + Kd = PilotDefinition.Conf.TireFollowingThkd, + DeadZone = PilotDefinition.Conf.TireFollowingThDeadZone, + SpeedAccPerSec = PilotDefinition.Conf.TireFollowingThSpeedAccPerSec, + OutputUpperThreshold = PilotDefinition.Conf.TireFollowingThThresh, + MaxI = PilotDefinition.Conf.TireFollowingThMaxI, + } + }.Get()).Wait(); + } + } + public class utils { public static List<(float x, float y, float th)> RemoveOutliers(List<(float x, float y, float th)> data, float threshold = 2.0f) diff --git a/MultiWheel/MultiWheelC/PilotConfig.cs b/MultiWheel/MultiWheelC/PilotConfig.cs index d341731..2a82bdd 100644 --- a/MultiWheel/MultiWheelC/PilotConfig.cs +++ b/MultiWheel/MultiWheelC/PilotConfig.cs @@ -306,6 +306,14 @@ public class PilotConfig : MultiWheelPilotConfig [FieldMember(desc = "杞儙璺熻釜:Y鏈澶у钩鍧囨暟")] public int TireFollowingYAverageFrameCount = 5; [FieldMember(desc = "缁堢偣璺熻釜:閫熷害")] public float DstTrackerMaxSpeed = 0.3f; - + [FieldMember(desc = "杞儙璺熻釜:閲婃斁閿佺偣璺濈")] public float TireFollowingReleaseDistance = 1600; #endregion + + [FieldMember(desc = "杞儙璺熻釜:瑙掑害璋冩暣kp")] public float TireFollowingThkp = 0.05f; + [FieldMember(desc = "杞儙璺熻釜:瑙掑害璋冩暣ki")] public float TireFollowingThki = 0.01f; + [FieldMember(desc = "杞儙璺熻釜:瑙掑害璋冩暣kd")] public float TireFollowingThkd = 0f; + [FieldMember(desc = "杞儙璺熻釜:瑙掑害璋冩暣SpeedAcc")] public float TireFollowingThSpeedAccPerSec = 1f; + [FieldMember(desc = "杞儙璺熻釜:瑙掑害璋冩暣Thresh")] public float TireFollowingThThresh = 0.1f; + [FieldMember(desc = "杞儙璺熻釜:瑙掑害璋冩暣DeadZone")] public float TireFollowingThDeadZone = 5f; + [FieldMember(desc = "杞儙璺熻釜:瑙掑害璋冩暣MaxI")] public float TireFollowingThMaxI = 0.01f; } diff --git a/MultiWheel/MultiWheelC/TireFollowing.cs b/MultiWheel/MultiWheelC/TireFollowing.cs index 766093d..049d572 100644 --- a/MultiWheel/MultiWheelC/TireFollowing.cs +++ b/MultiWheel/MultiWheelC/TireFollowing.cs @@ -297,7 +297,8 @@ namespace MultiWheelC if (EnableHandover) { controller.SlowDistance = float.MinValue; - controller.FinishDistance = 20; + controller.FinishSpeed = 0.2f; + controller.FinishDistance = 50; } (WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh) = GetCurrentPos2Dst(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh); var walkBlindPathEnd = Tuple.Create(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh); @@ -416,6 +417,14 @@ namespace MultiWheelC while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0); rd = _remainDistanceList.Average(); _painter.DrawText(Color.Green, $"{rd:F3}", distanceLabelPos.X, distanceLabelPos.Y - 200); + if(rd < PilotDefinition.Conf.TireFollowingReleaseDistance) + { + if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null) + { + detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId); + DLog.Log($"閲婃斁棰勫彇杞︾偣{detectors[detectorIndex].SrcId}", "TireFollowing"); + } + } if (detectorIndex < detectors.Count - 1) { @@ -423,11 +432,11 @@ namespace MultiWheelC if (detectors[detectorIndex].SwitchWalkBlindCondition(rd)) { WalkBlindStage1 = true; - if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null) - { - detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId); - DLog.Log($"閲婃斁棰勫彇杞︾偣{detectors[detectorIndex].SrcId}", "TireFollowing"); - } + //if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null) + //{ + // detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId); + // DLog.Log($"閲婃斁棰勫彇杞︾偣{detectors[detectorIndex].SrcId}", "TireFollowing"); + //} WalkBlindCarPathDstX = trackDst.X; WalkBlindCarPathDstY = trackDst.Y; WalkBlindCarPathDstTh = angle2target + WalkBlindTh;