fix 钻车结束速度不降到0的Bug

2026-07-03 13:32:26 +08:00
parent dadbcc76f0
commit fc7c8e243b
2 changed files with 15 additions and 4 deletions
@@ -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");
        }

@@ -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;
                        }
                    }