update 离车动作连贯

MultiWheel/MultiWheelC/TireFollowing.cs

@@ -50,6 +50,11 @@ namespace MultiWheelC
         /// </summary>
         public float MaxSpeed = 0.3f;
 
+        // 末段衔接：接近盲走终点时给非零速度，供后续动作连续接管
+        public bool EnableHandover = false;
+        public float HandoverDistance = 200f; // mm
+        public float HandoverSpeed = 0.2f;    // m/s
+
         /// <summary>
         /// 钻轮胎数量
         /// </summary>
@@ -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;
                         }
                     }