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

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");
         }
 

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