diff --git a/MultiWheel/MultiWheelC/AGV.cs b/MultiWheel/MultiWheelC/AGV.cs index 9775686..7763deb 100644 --- a/MultiWheel/MultiWheelC/AGV.cs +++ b/MultiWheel/MultiWheelC/AGV.cs @@ -152,7 +152,7 @@ namespace MultiWheelC } //离车一定是后雷达识别一个轮胎 - public void LeaveCar(int srcId, int dstId) + public void LeaveCar(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY) { while (!TryLock(dstId)) { @@ -192,6 +192,11 @@ namespace MultiWheelC }; var _dt = new DriveTask(following.Get()); _dt.Wait(); + new DriveTask(new DstTracker() + { + Src = new Vector2(srcX, srcY), + Dst = new Vector2(dstX, dstY), + }.Get()); Wait(); DLog.Log("钻车动作结束", "TireFollowing"); } diff --git a/MultiWheel/MultiWheelC/TireFollowing.cs b/MultiWheel/MultiWheelC/TireFollowing.cs index 4998ef7..547d674 100644 --- a/MultiWheel/MultiWheelC/TireFollowing.cs +++ b/MultiWheel/MultiWheelC/TireFollowing.cs @@ -200,7 +200,12 @@ namespace MultiWheelC controller.FinishDistance = float.MinValue; controller.FirstThAccuracy = 999; _dt = new DriveTask(controller.Track(true, CoordinateSystem.Car2D)); - + void HardStop() + { + _dt?.Stop(); + ((MultiWheelChassis)PilotDefinition.Chassis).DriveStop(); + DLog.Log($"Hard Stop!", "TireFollowing"); + } float WalkBlindCarPathDstX = -1f, WalkBlindCarPathDstY = -1f, WalkBlindCarPathDstTh = -1f; bool WalkBlindStage1 = false, WalkBlindStage2 = false; var angle2target = -1f; @@ -280,6 +285,7 @@ namespace MultiWheelC //绗簩娆$洸璧版椂鎴栧彧閽讳竴涓疆鑳庢椂 if (WalkBlindStage2 || detectors.Count == 1 || TireNum == 1) { + //controller.FinishDistance = 10f; controller.SlowDistance = SlowDistance; controller.SlowingPow = 0.7f; } @@ -320,14 +326,14 @@ namespace MultiWheelC detectorIndex++; if (detectors.Count == 1 || TireNum == 1) { - _dt.Stop(); + HardStop(); yield return false; } } else if (WalkBlindStage2) { DLog.Log("杈惧埌绗簩瀵硅疆鑳庡锛屽仠姝㈢Щ鍔", "TireFollowing"); - _dt.Stop(); + HardStop(); yield return false; } }