update 离车动作连贯

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