Update multi-vehicle sync and crab walk docs

MultiWheel/MultiWheelC/ChassisController.cs

@@ -3,6 +3,7 @@ using System.Numerics;
 using ClumsyCore;
 using ClumsyCore.Interfaces;
 using ClumsyCore.Pilot;
+using FundamentalLib;
 using MDCSToolBox.Clumsy.MotionControllers;
 using MDCSToolBox.Clumsy.Movements;
 using MDCSToolBox.Clumsy.Pilot;
@@ -13,6 +14,8 @@ public class ChassisController : MovementDefinition<MultiWheelGeometricControlle
 {
     public float BaseSpeed = Configuration.conf.basicSpeed;
 
+    private DateTime _sendMotionDbgLast = DateTime.MinValue;
+
     public override MultiWheelGeometricController Get()
     {
         return new MultiWheelGeometricController
@@ -45,25 +48,52 @@ public class ChassisController : MovementDefinition<MultiWheelGeometricControlle
 
             MultiVehicleSendMotion = (speed, frontTh, rearTh, idealPos, idealAngle) =>
             {
-                PilotDefinition.Self.MultiVehicleAutoEnabled = true;
-                lock (PilotDefinition.Self.MultiVehicleFleet)
+                var self = PilotDefinition.Self;
+                self.MultiVehicleAutoEnabled = true;
+                // A: 用固定锁对象（不再锁会被替换的字段引用）。
+                int fleetCnt;
+                lock (self.FleetLock)
+                    fleetCnt = self.MultiVehicleFleet.Count;
+
+                // 诊断（节流 ~300ms）：确认回调被调用、编队是否就绪、是否因数量不符提前 return（导致不下发速度）。
+                if ((DateTime.Now - _sendMotionDbgLast).TotalMilliseconds >= 300)
                 {
-                    if (PilotDefinition.Self.MultiVehicleFleet.Count != PilotDefinition.Conf.MultiVehicleFleetNum)
-                        return;
+                    _sendMotionDbgLast = DateTime.Now;
+                    DLog.Log(
+                        $"SENDMOTION speed={speed:0.000} fTh={frontTh:0.0} rTh={rearTh:0.0} " +
+                        $"ideal=({idealPos.X:0},{idealPos.Y:0},{idealAngle:0.0}) " +
+                        $"editCnt={fleetCnt}/{PilotDefinition.Conf.MultiVehicleFleetNum} " +
+                        $"earlyReturn={fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum}",
+                        "FleetCrabDbg");
                 }
 
-                PilotDefinition.Self.MultiVehicleAutoVx = speed;
-                PilotDefinition.Self.MultiVehicleAutoFrontTh = frontTh;
-                PilotDefinition.Self.MultiVehicleAutoRearTh = rearTh;
+                if (fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum)
+                    return;
+
+                self.MultiVehicleAutoVx = speed;
+                self.MultiVehicleAutoFrontTh = frontTh;
+                self.MultiVehicleAutoRearTh = rearTh;
+                // D: 透传路径控制器算出的理想车队中心位姿（此前被丢弃），供各车按 layout 做前馈。
+                self.MultiVehicleAutoIdealX = idealPos.X;
+                self.MultiVehicleAutoIdealY = idealPos.Y;
+                self.MultiVehicleAutoIdealTh = idealAngle;
+                self.MultiVehicleAutoHasIdeal = true;
+                // B: 标记命令新鲜度。路径结束/早退/卡顿不再刷新此时刻 → 主车超时后清零速度，避免滑行。
+                self.MultiVehicleAutoCmdTime = DateTime.Now;
             },
 
-            MultiVehicleGetFleetPos = () => new Location
+            // G: 读取车队中心原子快照，避免跨线程读到撕裂的 x/y/th 组合。
+            MultiVehicleGetFleetPos = () =>
             {
-                x = PilotDefinition.Self.CenterX,
-                y = PilotDefinition.Self.CenterY,
-                th = PilotDefinition.Self.CenterTh,
-                l_step = 1,
-                tick = DateTime.Now.Ticks
+                var snap = PilotDefinition.Self.GetFleetCenterSnapshot();
+                return new Location
+                {
+                    x = snap.X,
+                    y = snap.Y,
+                    th = snap.Th,
+                    l_step = 1,
+                    tick = DateTime.Now.Ticks
+                };
             }
         };
     }