diff --git a/MultiWheel/MultiWheelC/AGV.cs b/MultiWheel/MultiWheelC/AGV.cs index e857a63..07e0911 100644 --- a/MultiWheel/MultiWheelC/AGV.cs +++ b/MultiWheel/MultiWheelC/AGV.cs @@ -70,20 +70,20 @@ namespace MultiWheelC } } - //参数1:tireNum 需要钻过的轮胎对数量 - //参数2:frontLidarDetect true:前雷达识别 false:后雷达识别 + //1tireNum Ҫ̥ + //2frontLidarDetect true:ǰ״ʶ false:״ʶ public void TireFollowing(int tireNum, bool frontLidarDetect, int srcId, int dstId) { while (!TryLock(dstId)) { Thread.Sleep(50); } - DLog.Log($"锁点{dstId}完成", "TireFollowing"); - var lidarName = frontLidarDetect ? "前雷达" : "后雷达"; - DLog.Log($"开始钻车动作,通过{lidarName}识别结果钻{tireNum}对轮胎", "TireFollowing"); + DLog.Log($"{dstId}", "TireFollowing"); + var lidarName = frontLidarDetect ? "ǰ״" : "״"; + DLog.Log($"ʼ공ͨ{lidarName}ʶ{tireNum}̥", "TireFollowing"); if (tireNum != 1 && tireNum != 2) { - DLog.Log($"TireNum必须是1或2 (当前输入:{tireNum})", "TireFollowing"); + DLog.Log($"TireNum12 (ǰ:{tireNum})", "TireFollowing"); return; } @@ -91,20 +91,20 @@ namespace MultiWheelC { while (!TryLock(dstId)) { - Console.WriteLine("等待锁取货点中..."); + Console.WriteLine("ȴȡ..."); Thread.Sleep(200); } - Console.WriteLine($"锁点{dstId}完成"); + Console.WriteLine($"{dstId}"); Thread.Sleep(1000); - Console.WriteLine($"开始钻车动作,通过{lidarName}识别结果钻{tireNum}对轮胎"); + Console.WriteLine($"ʼ공ͨ{lidarName}ʶ{tireNum}̥"); Thread.Sleep(1000); Leave(srcId); - Console.WriteLine($"开始第一段盲走,此时释放预取货点{srcId}"); + Console.WriteLine($"ʼһäߣʱͷԤȡ{srcId}"); Thread.Sleep(2000); //Leave(dstId); - //Console.WriteLine($"结束第一段盲走,此时释放取货点{dstId}"); + //Console.WriteLine($"һäߣʱͷȡ{dstId}"); Thread.Sleep(2000); - Console.WriteLine($"结束钻车动作"); + Console.WriteLine($"공"); return; } @@ -138,7 +138,7 @@ namespace MultiWheelC 0) }, }; - DLog.Log($"钻胎为{tireNum}", "TireFollowing"); + DLog.Log($"̥Ϊ{tireNum}", "TireFollowing"); var following = new TireFollowing() { GetController = () => new ChassisController().Get(), @@ -153,18 +153,18 @@ namespace MultiWheelC }; var _dt = new DriveTask(following.Get()); _dt.Wait(); - DLog.Log("钻车动作结束", "TireFollowing"); + DLog.Log("공", "TireFollowing"); } - //离车一定是后雷达识别一个轮胎 + //복һǺ״ʶһ̥ public void LeaveCar(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY) { while (!TryLock(dstId)) { Thread.Sleep(50); } - DLog.Log($"锁点{dstId}完成", "TireFollowing"); - DLog.Log($"开始钻车动作,通过后雷达识别结果钻1对轮胎", "TireFollowing"); + DLog.Log($"{dstId}", "TireFollowing"); + DLog.Log($"ʼ공ͨ״ʶ1̥", "TireFollowing"); var chassis = (MultiWheelChassis)PilotDefinition.Chassis; chassis.SetOriginBias(0, 0, 0); var following = new TireFollowing() @@ -193,6 +193,9 @@ namespace MultiWheelC CarDirection = 180f, SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance, MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed, + EnableHandover = true, + HandoverDistance = 200f, + HandoverSpeed = 0.3f, WalkBlindTh = 0, TireNum = 1 }; @@ -204,14 +207,15 @@ namespace MultiWheelC if (!running) break; yield return true; } - DLog.Log($"释放锁点{srcId}完成", "TireFollowing"); - DLog.Log("离车TireFollowing结束,开始DstTracker", "TireFollowing"); + DLog.Log($"ͷ{srcId}", "TireFollowing"); + DLog.Log("복TireFollowingʼDstTracker", "TireFollowing"); foreach (var running in new DstTracker() { Src = new Vector2(srcX, srcY), Dst = new Vector2(dstX, dstY), CarDirectionBias = 180f, + InitialSendSpeed = 0.3f }.Get()) { if (!running) break; @@ -223,7 +227,7 @@ namespace MultiWheelC var _dt = new DriveTask(LeaveThenFollow()); _dt.Wait(); - DLog.Log("离车动作1结束", "TireFollowing"); + DLog.Log("복1", "TireFollowing"); } public void LineTracking(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY) @@ -234,7 +238,7 @@ namespace MultiWheelC } var chassis = (MultiWheelChassis)PilotDefinition.Chassis; chassis.SetOriginBias(0, 0, 0); - DLog.Log($"锁点{dstId}完成", "TireFollowing"); + DLog.Log($"{dstId}", "TireFollowing"); IEnumerable TrackThenFollow() { foreach (var running in new LineTracking() @@ -247,11 +251,13 @@ namespace MultiWheelC if (!running) break; yield return true; } - DLog.Log("离车LineTracking结束,开始DstTracker", "TireFollowing"); + DLog.Log($"ͷ{srcId}", "TireFollowing"); + DLog.Log("복LineTrackingʼDstTracker", "TireFollowing"); foreach (var running in new DstTracker() { Src = new Vector2(srcX, srcY), Dst = new Vector2(dstX, dstY), + InitialSendSpeed = 0.3f }.Get()) { if (!running) break; @@ -261,34 +267,32 @@ namespace MultiWheelC } var _dt = new DriveTask(TrackThenFollow()); _dt.Wait(); - //Leave(srcId); - DLog.Log($"释放锁点{srcId}完成", "TireFollowing"); - DLog.Log("离车动作2结束", "TireFollowing"); + DLog.Log("복2", "TireFollowing"); } - //驱动器上使能 + //ʹ public void DriverAble() { var dl = new DriveTask(new DriverAble() { }.Get()); dl.Wait(); - DLog.Log("驱动器上使能完成", "TireFollowing"); + DLog.Log("ʹ", "TireFollowing"); } - //驱动器下使能 + //ʹ public void DriverDisable() { var dl = new DriveTask(new DriverDisable() { }.Get()); dl.Wait(); - DLog.Log("驱动器下使能完成", "TireFollowing"); + DLog.Log("ʹ", "TireFollowing"); } - // 夹抱:close 为 true 时关闭夹抱,否则打开夹抱。 + // бclose Ϊ true ʱرռб򿪼б public void ClamptoTarget(bool close) { if (PilotDefinition.Self.GhostMode) { Thread.Sleep(2000); - Console.WriteLine("夹抱完成"); + Console.WriteLine("б"); return; } new DriveTask(new ClampToTarget() @@ -334,7 +338,7 @@ namespace MultiWheelC { Thread.Sleep(50); } - DLog.Log($"锁点{dstId}完成", "FleetCrabDbg"); + DLog.Log($"{dstId}", "FleetCrabDbg"); } var action = new MultiWheelC.FleetCrabWalk @@ -360,7 +364,7 @@ namespace MultiWheelC if (srcId != -1) { Leave(srcId); - DLog.Log($"释放放车点{srcId}", "FleetCrabDbg"); + DLog.Log($"ͷŷų{srcId}", "FleetCrabDbg"); } } } diff --git a/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs b/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs index 6f6c55a..7c60848 100644 --- a/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs +++ b/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs @@ -385,6 +385,9 @@ namespace MultiWheelC foreach (var running in new LineTracking() { Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2, + EnableHandover = true, + HandoverDistance = 200f, + HandoverSpeed = 0.3f, }.Get()) { if (!running) break; @@ -395,6 +398,7 @@ namespace MultiWheelC Src = src, Dst = dst, CarDirectionBias = carDirectionBias, + InitialSendSpeed = 0.3f }.Get()) { if (!running) break; diff --git a/MultiWheel/MultiWheelC/Movements.cs b/MultiWheel/MultiWheelC/Movements.cs index 0998960..6dd4a7c 100644 --- a/MultiWheel/MultiWheelC/Movements.cs +++ b/MultiWheel/MultiWheelC/Movements.cs @@ -176,15 +176,23 @@ namespace MultiWheelC public float CarDirectionBias = 0f; public Painter Painter = UI.GetPainter("DstTracker"); + public float InitialSendSpeed = 0; + 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(); + + if (InitialSendSpeed != 0) + { + tracker.SkipInitialRotate = true; + tracker.InitialSendSpeed = InitialSendSpeed; + } + var linePath = new LineTrack(Src, Dst) { CarDirectionBias = CarDirectionBias, Speed = PilotDefinition.Conf.DstTrackerMaxSpeed }; tracker.AddTrack(linePath); - var task = new DriveTask(tracker.Track()); task.Wait(); @@ -208,6 +216,10 @@ namespace MultiWheelC public int DstId = -1; public Action LeaveSrcFunction = null; private PIDController pid; + // 末段衔接:接近目标后不再让 PID 把速度降到 0,保留一个接力速度给后续动作接管 + public bool EnableHandover = false; + public float HandoverDistance = 80f; // mm + public float HandoverSpeed = 0.15f; // m/s public override IEnumerable Get() { @@ -222,6 +234,19 @@ namespace MultiWheelC DLog.Log($"直线行驶距离:{Target}", "TireFollowing"); while (true) { + var current = (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2; + var remain = Target - current; + if (EnableHandover && Math.Abs(remain) <= Math.Max(1f, HandoverDistance)) + { + var handoverSign = Math.Sign(remain); + if (handoverSign == 0) handoverSign = 1; + var handoverSpeed = Math.Abs(HandoverSpeed) * handoverSign; + Console.WriteLine($"handover speed: {handoverSpeed:F3}, remain: {remain:F2}"); + chassis.SendXYThSpeed(handoverSpeed, 0, 0); + // 保留一拍接力速度,让后续 DstTracker 无缝接管 + yield return true; + break; + } var speed = pid.GetResponse(Target); Console.WriteLine($"output: {speed} current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}"); diff --git a/MultiWheel/MultiWheelC/TireFollowing.cs b/MultiWheel/MultiWheelC/TireFollowing.cs index 547d674..766093d 100644 --- a/MultiWheel/MultiWheelC/TireFollowing.cs +++ b/MultiWheel/MultiWheelC/TireFollowing.cs @@ -50,6 +50,11 @@ namespace MultiWheelC /// public float MaxSpeed = 0.3f; + // 末段衔接:接近盲走终点时给非零速度,供后续动作连续接管 + public bool EnableHandover = false; + public float HandoverDistance = 200f; // mm + public float HandoverSpeed = 0.2f; // m/s + /// /// 钻轮胎数量 /// @@ -289,6 +294,11 @@ namespace MultiWheelC controller.SlowDistance = SlowDistance; controller.SlowingPow = 0.7f; } + if (EnableHandover) + { + controller.SlowDistance = float.MinValue; + controller.FinishDistance = 20; + } (WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh) = GetCurrentPos2Dst(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh); var walkBlindPathEnd = Tuple.Create(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh); var walkBlindPathStart = LessMath.Transform2D(walkBlindPathEnd, Tuple.Create(CarDirection == 0 ? -3000f : 3000f, 0f, 0f)); @@ -324,7 +334,7 @@ namespace MultiWheelC _remainAngleList.Clear(); _remainDistanceList.Clear(); detectorIndex++; - if (detectors.Count == 1 || TireNum == 1) + if ((detectors.Count == 1 || TireNum == 1) && !EnableHandover) { HardStop(); yield return false; @@ -333,7 +343,10 @@ namespace MultiWheelC else if (WalkBlindStage2) { DLog.Log("达到第二对轮胎处,停止移动", "TireFollowing"); - HardStop(); + if (!EnableHandover) + { + HardStop(); + } yield return false; } }