Update MultiWheel fleet sync and restore clamp support

MultiWheel/MultiWheelC/AGV.cs

@@ -210,6 +210,21 @@ namespace MultiWheelC
             DLog.Log("驱动器下使能完成", "TireFollowing");
         }
 
+        // 夹抱：close 为 true 时关闭夹抱，否则打开夹抱。
+        public void ClamptoTarget(bool close)
+        {
+            if (PilotDefinition.Self.GhostMode)
+            {
+                Thread.Sleep(2000);
+                Console.WriteLine("夹抱完成");
+                return;
+            }
+            new DriveTask(new ClampToTarget()
+            {
+                LeftClampTarget = close ? PilotDefinition.Self.LeftArmUpperPos : PilotDefinition.Self.LeftArmLowerPos,
+                RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos
+            }.Get()).Wait();
+        }
         public void LineTracking(int srcId, int dstId, float LineDistance)
         {
             while (!TryLock(dstId))

MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs

@@ -210,6 +210,52 @@ namespace MultiWheelC
         private DriveTask _dt;
     }
 
+    [MovementTest(name = "抱夹关闭")]
+    public class ClampTest1 : MovementTest
+    {
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            PilotDefinition.Self.SpeedLeftArm = 0;
+            PilotDefinition.Self.SpeedRightArm = 0;
+        }
+
+        public override void Test()
+        {
+            _dt = new DriveTask(new ClampToTarget()
+            {
+                LeftClampTarget = PilotDefinition.Self.LeftArmUpperPos,
+                RightClampTarget = PilotDefinition.Self.RightArmUpperPos
+            }.Get());
+            _dt.Wait();
+        }
+
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "抱夹打开")]
+    public class ClampTest2 : MovementTest
+    {
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            PilotDefinition.Self.SpeedLeftArm = 0;
+            PilotDefinition.Self.SpeedRightArm = 0;
+        }
+
+        public override void Test()
+        {
+            _dt = new DriveTask(new ClampToTarget()
+            {
+                LeftClampTarget = PilotDefinition.Self.LeftArmLowerPos,
+                RightClampTarget = PilotDefinition.Self.RightArmLowerPos
+            }.Get());
+            _dt.Wait();
+        }
+
+        private DriveTask _dt;
+    }
+
     [MovementTest(name = "测试前进基于轮里程")]
     public class LineTrackingTest : MovementTest
     {

MultiWheel/MultiWheelC/Movements.cs

@@ -73,6 +73,50 @@ namespace MultiWheelC
     }
 
 
+    public class ClampToTarget : MovementDefinition
+    {
+        public float LeftClampTarget;
+        public float RightClampTarget;
+
+        public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed;
+        public float ClampKp = PilotDefinition.Conf.ClampControlKp;
+        public float ClampKi = PilotDefinition.Conf.ClampControlKi;
+        public float ClampKd = PilotDefinition.Conf.ClampControlKd;
+        public float ClampMaxI = PilotDefinition.Conf.ClampControlMaxI;
+        public float ClampSpeedAcc = PilotDefinition.Conf.ClampControlSpeedAcc;
+        public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
+        private PIDController leftpid, rightpid;
+
+        public override IEnumerable<bool> Get()
+        {
+            leftpid = new PIDController(() => PilotDefinition.Self.ActualPosLeftArm, ClampKp, ClampKi, ClampKd,
+                    ClampMaxI, ClampDeadZone, MaxClampSpeed)
+                { SpeedAccPerSec = ClampSpeedAcc };
+
+            rightpid = new PIDController(() => PilotDefinition.Self.ActualPosRightArm, ClampKp, ClampKi, ClampKd,
+                    ClampMaxI, ClampDeadZone, MaxClampSpeed)
+                { SpeedAccPerSec = ClampSpeedAcc };
+
+            while (true)
+            {
+                var leftspeed = leftpid.GetResponse(LeftClampTarget);
+                var rightspeed = rightpid.GetResponse(RightClampTarget);
+                Console.WriteLine($"left arm speed:{leftspeed}  right arm speed:{rightspeed}");
+                PilotDefinition.Self.SpeedLeftArm = leftspeed;
+                PilotDefinition.Self.SpeedRightArm = rightspeed;
+                if (leftpid.IsArrived()) PilotDefinition.Self.SpeedLeftArm = 0;
+                if (rightpid.IsArrived()) PilotDefinition.Self.SpeedRightArm = 0;
+
+                if (leftpid.IsArrived() && rightpid.IsArrived()) break;
+                yield return true;
+            }
+
+            PilotDefinition.Self.SpeedLeftArm = 0;
+            PilotDefinition.Self.SpeedRightArm = 0;
+            Console.WriteLine($"left clamp to target:{LeftClampTarget}  right clamp to target:{RightClampTarget}");
+        }
+    }
+
     public class Sleep : MovementDefinition
     {
         public float Second = 2;

MultiWheel/MultiWheelC/PilotConfig.cs

@@ -12,7 +12,8 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "[sync] 编队布局偏角(deg)")] public float TestCarSyncTh = 0f;
     // 手动遥控 Vx 已是 m/s、Vth 已是转向角(deg)，此处系数保持 1（直通），不要再次缩放。
     [FieldMember(desc = "[sync] 手动Vx系数")] public float ManualCarSyncVxFac = 1f;
-    [FieldMember(desc = "[sync] 手动Vy系数(蟹行横向)")] public float ManualCarSyncVyFac = 1f;
+    // Manual crab mode: VxFac scales linear speed, VyFac maps steer stick ratio to crab steer angle in degrees.
+    [FieldMember(desc = "[sync] 手动Vy系数(蟹行满杆舵角deg)")] public float ManualCarSyncVyFac = 60f;
     [FieldMember(desc = "[sync] 手动Vth系数")] public float ManualCarSyncVthFac = 1f;
     [FieldMember(desc = "[sync] 蟹行舵角上限(deg，应与Medulla舵轮角度限制匹配，默认120)")] public float MultiVehicleCrabSteerLimitDeg = 120f;
     [FieldMember(desc = "[sync] 检测中心偏移(mm)")] public float DeltaDetectCenter = 350f;
@@ -238,6 +239,15 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackTwoLegSgnDir = 1;
     [FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegCenterChangeX = 0;
 
+    [FieldMember(desc = "抱夹控制pid:Kp")] public float ClampControlKp = 0.1f;
+    [FieldMember(desc = "抱夹控制pid:Ki")] public float ClampControlKi = 0f;
+    [FieldMember(desc = "抱夹控制pid:Kd")] public float ClampControlKd = 0f;
+    [FieldMember(desc = "抱夹控制pid:MaxI")] public float ClampControlMaxI = 0f;
+    [FieldMember(desc = "抱夹控制pid:Acc")] public float ClampControlSpeedAcc = 1f;
+    [FieldMember(desc = "抱夹控制pid:Thresh")] public float ClampControlThresh = 0.2f;
+    [FieldMember(desc = "抱夹控制pid:DeadZone")] public float ClampControlDeadZone = 5f;
+    [FieldMember(desc = "抱夹最大速度")] public float MaxClampSpeed = 1.5f;
+
     [FieldMember(desc = "直线行走距离")] public float LineTrackDistance = 1000f;
     [FieldMember(desc = "直线行走最大速度")] public float LineTrackMaxSpeed = 0.3f;
     [FieldMember(desc = "直线行走Kp")] public float LineTrackKp = 0.2f;

MultiWheel/MultiWheelC/PilotDefinition.cs

@@ -100,6 +100,18 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
 
     [AsLowerIO(desc = "车号")] public int CarNum = 1;
 
+    #region 夹臂变量
+    [AsUpperIO(desc = "左夹臂下发速度")] public float SpeedLeftArm;
+
+    [AsUpperIO(desc = "右夹臂下发速度")] public float SpeedRightArm;
+
+    [AsLowerIO(desc = "左夹臂实际位置")] public float ActualPosLeftArm;
+
+    [AsLowerIO(desc = "右夹臂实际位置")] public float ActualPosRightArm;
+
+    [AsUpperIO(desc = "夹臂不同步报警")] public bool ClampOutOfSync = false;
+    #endregion
+
     [AsLowerIO(desc = "左前左轮实际位置")] public float LFLActualPos;
     [AsLowerIO(desc = "左前右轮实际位置")] public float LFRActualPos;
     [AsLowerIO(desc = "右前左轮实际位置")] public float RFLActualPos;
@@ -109,6 +121,11 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
     [AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
     [AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;
 
+    [AsLowerIO(desc = "左夹臂低限位")] public float LeftArmLowerPos;
+    [AsLowerIO(desc = "左夹臂高限位")] public float LeftArmUpperPos;
+    [AsLowerIO(desc = "右夹臂低限位")] public float RightArmLowerPos;
+    [AsLowerIO(desc = "右夹臂高限位")] public float RightArmUpperPos;
+
     [AsUpperIO(desc = "从C往驱动器下使能")] public bool DisableFromC = false;
     [AsUpperIO(desc = "从C上复位")] public bool ResetFromC = false;
 
@@ -117,6 +134,10 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
     private readonly object _multiVehicleNotificationLock = new();
     private DateTime _multiVehicleLastNotifyTime = DateTime.MinValue;
     private bool _multiVehicleSyncInitialized;
+    private bool _multiVehicleWasActive;
+    private bool _multiVehicleMotionFeasible = true;
+    private string _multiVehicleMotionInfeasibleReason = "";
+    private DateTime _multiVehicleStopLastLog = DateTime.MinValue;
 
     // 诊断日志：节流计时 + 最近一次检测几何（中心/朝向/距离），用于定位剧烈运动来源
     private DateTime _mvDbgLastLog = DateTime.MinValue;
@@ -177,6 +198,23 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         DLog.Log($"car{CarNum} {msg}", "MultiVehicleRemoteDbg");
     }
 
+    private void SetMultiVehicleMotionFeasible(bool feasible, string reason = "")
+    {
+        _multiVehicleMotionFeasible = feasible;
+        _multiVehicleMotionInfeasibleReason = feasible ? "" : (reason ?? "");
+    }
+
+    private void LogMultiVehicleStop(string reason, bool force = false)
+    {
+        var now = DateTime.Now;
+        if (!force && (now - _multiVehicleStopLastLog).TotalMilliseconds < 500) return;
+        _multiVehicleStopLastLog = now;
+        var msg = $"FLEET_STOP car={CarNum} reason={reason}";
+        DLog.Log(msg, "MultiVehicleSafety");
+        FleetDiag(msg);
+        Hedingben.ToastText($"Fleet stop: {reason}", $"MultiVehicle{CarNum}-stop");
+    }
+
     private readonly object _neighborDetectLock = new();
     private readonly List<(DateTime Time, Vector2 Src, Vector2 Dst)> _neighborDetects = new();
 
@@ -427,6 +465,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
 
         if (!manualEnabled && !autoEnabled)
         {
+            if (_multiVehicleWasActive)
+            {
+                chassis.RampStop();
+                SetMultiVehicleMotionFeasible(true);
+                LogMultiVehicleStop("fleet control disabled; ramp stop previous fleet command", true);
+            }
+            _multiVehicleWasActive = false;
             LogMultiVehicleRemoteDecision(
                 $"RETURN_IDLE master={isMaster} rawEn={MultiVehicleManualEnabled} scriptOn={scriptOn} auto={autoEnabled} " +
                 $"rawMode={MultiVehicleManualMode} rawVx={MultiVehicleManualVx:0.000} rawVy={MultiVehicleManualVy:0.000} rawVth={MultiVehicleManualVth:0.000}");
@@ -448,11 +493,15 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
 
             return;
         }
+        _multiVehicleWasActive = true;
 
         // 自动模式整队姿态依赖 Detour 全局定位（与 MultiVehicleSyncUseDetour 无关）：要求编队至少一台车有定位。
         if (isMaster && autoEnabled && !manualEnabled && !FleetHasPosAvailable())
         {
             MultiVehicleAutoEnabled = false;
+            chassis.RampStop();
+            SetMultiVehicleMotionFeasible(true);
+            LogMultiVehicleStop("auto mode requires at least one Detour-positioned fleet member", true);
             Hedingben.ToastText("自动多车联动需要至少一台车有 Detour 定位", "MultiVehicle-auto-gate");
             return;
         }
@@ -482,6 +531,10 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         var deltaDetectCenter = Conf.DeltaDetectCenter;
         float fleetVx = 0, fleetFrontTh = 0, fleetRearTh = 0, fleetOmega = 0;
         var fleetMode = 0; // 0=常规 1=蟹行 2=原地旋转
+        var autoCommandTimedOut = false;
+        var notificationStopActive = false;
+        var notificationStopReason = "";
+        var notificationStopSourceCar = 0;
         float crabInputVx = 0, crabInputVy = 0, crabRawAngle = 0;
         float crabSteerLimit = 0;
         var crabReverseEquivalent = false;
@@ -504,15 +557,12 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 }
                 else if (fleetMode == 1)
                 {
-                    // 蟹行：把 (前后向Vx, 横向Vy) 合成速度矢量，四轮同向打到该方向（前后舵轮角相同）。
-                    // 舵轮物理/仿真限制约为 ±120°，不要把 ±90° 当边界；否则纯横移附近会在
-                    // -90° 与 +90°/反向速度两种等价表示之间跳变。只有超过蟹行舵角上限时才取等价反向。
-                    var vx = manualVx * Conf.ManualCarSyncVxFac;
-                    var vy = manualVy * Conf.ManualCarSyncVyFac;
-                    var speed = (float)Math.Sqrt(vx * vx + vy * vy);
-                    var crabAngle = (float)(Math.Atan2(vy, vx) * 180.0 / Math.PI);
-                    crabInputVx = vx;
-                    crabInputVy = vy;
+                    // 手动蟹行：Vx 只表示线速度，Vy 表示方向摇杆比例(-1..1)，由 VyFac 映射为舵角。
+                    var speed = manualVx * Conf.ManualCarSyncVxFac;
+                    var crabRatio = Math.Max(-60f, Math.Min(60f, manualVy));
+                    var crabAngle = crabRatio * Conf.ManualCarSyncVyFac;
+                    crabInputVx = speed;
+                    crabInputVy = crabRatio;
                     crabRawAngle = crabAngle;
                     crabSteerLimit = Math.Min(179f, Math.Max(1f, Math.Abs(Conf.MultiVehicleCrabSteerLimitDeg)));
 
@@ -554,6 +604,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 }
                 else
                 {
+                    autoCommandTimedOut = true;
                     fleetVx = fleetFrontTh = fleetRearTh = 0;
                     MultiVehicleAutoVx = MultiVehicleAutoFrontTh = MultiVehicleAutoRearTh = 0;
                     MultiVehicleAutoHasIdeal = false;
@@ -580,6 +631,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             fleetRearTh = notification.FleetRearTh;
             fleetMode = notification.Mode;
             fleetOmega = notification.FleetOmega;
+            notificationStopActive = notification.FleetStopActive;
+            notificationStopReason = notification.FleetStopReason ?? "";
+            notificationStopSourceCar = notification.FleetStopSourceCar;
             // D: 从车采用主车广播的理想车队中心（弧线时由 idealPos/idealAngle 而来）做前馈目标。
             MultiVehicleAutoHasIdeal = notification.HasIdeal;
             MultiVehicleAutoIdealX = notification.IdealX;
@@ -648,13 +702,67 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                                   MultiVehicleFleet.Values.All(v => v.Aligned);
         }
 
+        var fleetReady = false;
+        var fleetCount = 0;
+        lock (FleetLock)
+        {
+            fleetCount = MultiVehicleFleet.Count;
+            fleetReady = fleetCount == Conf.MultiVehicleFleetNum;
+        }
+
         // 安全门：开启互识别时，本车或任一其它车检测不到邻车则整队停车（速度置零）。
         // ownDetectOk 是本轮新鲜值；其它车的 DetectOk 来自其上报/主车下发（滑动窗口已给 1s 去抖）。
         var ownDetectOk = !Conf.MultiVehicleUseDetect || detectValid;
         bool othersDetectOk;
+        string otherDetectLostCars;
+        List<KeyValuePair<int, VehicleSyncInfo>> motionInfeasibleMembers;
         lock (FleetLock)
+        {
             othersDetectOk = MultiVehicleFleet.Where(kv => kv.Key != CarNum).All(kv => kv.Value.DetectOk);
-        var canMove = !Conf.MultiVehicleUseDetect || (ownDetectOk && othersDetectOk);
+            otherDetectLostCars = string.Join(",", MultiVehicleFleet
+                .Where(kv => kv.Key != CarNum && !kv.Value.DetectOk)
+                .Select(kv => kv.Key.ToString()));
+            motionInfeasibleMembers = MultiVehicleFleet
+                .Where(kv => kv.Key != CarNum && !kv.Value.MotionFeasible)
+                .ToList();
+        }
+
+        var fleetStopActive = false;
+        var fleetStopReason = "";
+        var fleetStopSourceCar = 0;
+        void AddFleetStop(string reason, int sourceCar = 0)
+        {
+            if (string.IsNullOrWhiteSpace(reason)) return;
+            if (!fleetStopActive)
+            {
+                fleetStopReason = reason;
+                fleetStopSourceCar = sourceCar;
+            }
+            else
+            {
+                fleetStopReason += " | " + reason;
+                if (fleetStopSourceCar == 0) fleetStopSourceCar = sourceCar;
+            }
+            fleetStopActive = true;
+        }
+
+        if (!fleetReady)
+            AddFleetStop($"member not ready/stale fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum}");
+        if (autoCommandTimedOut)
+            AddFleetStop("auto command timeout");
+        if (Conf.MultiVehicleUseDetect && !ownDetectOk)
+            AddFleetStop("own two-leg detection lost", CarNum);
+        if (Conf.MultiVehicleUseDetect && !othersDetectOk)
+            AddFleetStop($"other two-leg detection lost cars=[{otherDetectLostCars}]");
+        if (notificationStopActive)
+            AddFleetStop($"master stop from car{notificationStopSourceCar}: {notificationStopReason}",
+                notificationStopSourceCar);
+        if (!_multiVehicleMotionFeasible)
+            AddFleetStop($"car{CarNum} motion infeasible: {_multiVehicleMotionInfeasibleReason}", CarNum);
+        foreach (var kv in motionInfeasibleMembers)
+            AddFleetStop($"car{kv.Key} motion infeasible: {kv.Value.MotionInfeasibleReason}", kv.Key);
+
+        var canMove = !fleetStopActive;
 
         // H: 自动模式（非手动）必须有有效车队中心——主车由 SLAM 反推、从车依赖主车广播 fleetPosValid。
         //    自动模式整队姿态始终依赖 Detour（与 MultiVehicleSyncUseDetour 无关）：定位全程丢失时强制停车。
@@ -665,6 +773,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             var fleetCenterValid = fleetPosValid && (!isMaster || TryInferFleetCenter(out _, out _, out _));
             if (!fleetCenterValid)
             {
+                AddFleetStop("auto fleet center invalid/localization lost", CarNum);
                 canMove = false;
                 Hedingben.ToastText("自动模式无有效车队中心（定位丢失），已停车", $"MultiVehicle{CarNum}-autostop");
             }
@@ -685,15 +794,10 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             Hedingben.ToastText("车队检测正常", $"MultiVehicle{CarNum}-stop");
         }
 
-        VisualizeFleet(contour, layoutX, layoutY, layoutTh);
+        if (fleetStopActive)
+            LogMultiVehicleStop(fleetStopReason);
 
-        var fleetReady = false;
-        var fleetCount = 0;
-        lock (FleetLock)
-        {
-            fleetCount = MultiVehicleFleet.Count;
-            fleetReady = fleetCount == Conf.MultiVehicleFleetNum;
-        }
+        VisualizeFleet(contour, layoutX, layoutY, layoutTh);
 
         // 补偿量提到块外，便于诊断日志统一记录三类来源（检测/SLAM）的贡献。
         float xDetectCompensate = 0, yDetectCompensate = 0, thDetectCompensate = 0;
@@ -706,7 +810,16 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 $"fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum} canMove={canMove} mode={fleetMode} " +
                 $"cmdVx={fleetVx:0.000} cmdFTh={fleetFrontTh:0.00} cmdRTh={fleetRearTh:0.00} omega={fleetOmega:0.000}");
 
-        if (fleetReady)
+        if (fleetStopActive)
+        {
+            chassis.RampStop();
+            SetMultiVehicleMotionFeasible(true);
+            LogMultiVehicleRemoteDecision(
+                $"RAMP_STOP master={isMaster} manual={manualEnabled} auto={autoEnabled} ready={fleetReady} " +
+                $"reason={fleetStopReason}", true);
+        }
+
+        if (fleetReady && !fleetStopActive)
         {
             chassis.SetOriginBias(layoutX, layoutY, layoutTh);
             // E: 统一控制点半径——配置 >0 用配置值，否则取 syncDistance/2（与编队几何一致），不再硬编码 510。
@@ -813,10 +926,18 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 _mvRotCompVy = rotCompVy;
                 _mvRotCompOmega = rotCompOmega;
 
-                chassis.SendRotateMotion(fleetOmega,
+                var motionOk = chassis.SendRotateMotion(fleetOmega,
                     localCompensateX: rotCompVx, localCompensateY: rotCompVy, localCompensateTh: rotCompOmega);
+                SetMultiVehicleMotionFeasible(motionOk, motionOk ? "" : chassis.LastMotionDecomposeFailureReason);
+                if (!motionOk)
+                {
+                    AddFleetStop($"car{CarNum} chassis rotate infeasible: {chassis.LastMotionDecomposeFailureReason}",
+                        CarNum);
+                    canMove = false;
+                    LogMultiVehicleStop(fleetStopReason, true);
+                }
                 LogMultiVehicleRemoteDecision(
-                    $"SEND_ROTATE master={isMaster} manual={manualEnabled} canMove={canMove} ready={fleetReady} " +
+                    $"SEND_ROTATE master={isMaster} manual={manualEnabled} canMove={canMove} ready={fleetReady} ok={motionOk} " +
                     $"mode={fleetMode} omega={fleetOmega:0.000} comp=({rotCompVx:0.0},{rotCompVy:0.0},{rotCompOmega:0.000}) " +
                     $"fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum}");
 
@@ -835,12 +956,20 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 _rotPoseEpisode = false;
                 _rotPosePrevTime = DateTime.MinValue;
                 // 常规/蟹行：蟹行时 frontTh==rearTh（四轮同向）即为平移，与常规共用同一下发路径。
-                chassis.SendMotion(fleetVx, fleetFrontTh, fleetRearTh, localControlRadius: controlRadius,
+                var motionOk = chassis.SendMotion(fleetVx, fleetFrontTh, fleetRearTh, localControlRadius: controlRadius,
                     localCompensateX: xDetectCompensate + xPosCompensate,
                     localCompensateY: yDetectCompensate + yPosCompensate,
                     localCompensateTh: thDetectCompensate + thPosCompensate);
+                SetMultiVehicleMotionFeasible(motionOk, motionOk ? "" : chassis.LastMotionDecomposeFailureReason);
+                if (!motionOk)
+                {
+                    AddFleetStop($"car{CarNum} chassis motion infeasible: {chassis.LastMotionDecomposeFailureReason}",
+                        CarNum);
+                    canMove = false;
+                    LogMultiVehicleStop(fleetStopReason, true);
+                }
                 LogMultiVehicleRemoteDecision(
-                    $"SEND_MOTION master={isMaster} manual={manualEnabled} canMove={canMove} ready={fleetReady} " +
+                    $"SEND_MOTION master={isMaster} manual={manualEnabled} canMove={canMove} ready={fleetReady} ok={motionOk} " +
                     $"mode={fleetMode} vx={fleetVx:0.000} fTh={fleetFrontTh:0.00} rTh={fleetRearTh:0.00} " +
                     $"comp=({xDetectCompensate + xPosCompensate:0.0},{yDetectCompensate + yPosCompensate:0.0},{thDetectCompensate + thPosCompensate:0.000}) " +
                     $"fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum}");
@@ -859,12 +988,12 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             var cy = yDetectCompensate + yPosCompensate;
             var cth = thDetectCompensate + thPosCompensate;
             var crabDbg = isMaster && manualEnabled && fleetMode == 1
-                ? $"| CRAB in({crabInputVx:F3},{crabInputVy:F3}) raw:{crabRawAngle:F2} limit:{crabSteerLimit:F1} rev:{crabReverseEquivalent} "
+                ? $"| CRAB speed:{crabInputVx:F3} steerRatio:{crabInputVy:F3} raw:{crabRawAngle:F2} limit:{crabSteerLimit:F1} rev:{crabReverseEquivalent} "
                 : "";
 
             var dbg =
                 $"car{CarNum} master:{isMaster} manual:{manualEnabled} auto:{autoEnabled} slam:{slamRead} corr:{useDetourCorrection} fleetPos:{fleetPosValid} " +
-                $"ready:{fleetReady}({fleetCount}/{Conf.MultiVehicleFleetNum}) canMove:{canMove} useDetect:{Conf.MultiVehicleUseDetect} " +
+                $"ready:{fleetReady}({fleetCount}/{Conf.MultiVehicleFleetNum}) canMove:{canMove} stop:{fleetStopActive} stopReason:{fleetStopReason} useDetect:{Conf.MultiVehicleUseDetect} " +
                 $"| BASE vx:{fleetVx:F3} fTh:{fleetFrontTh:F2} rTh:{fleetRearTh:F2} " +
                 crabDbg +
                 $"| DETECT valid:{detectValid} center({_mvLastDetCenterX:F0},{_mvLastDetCenterY:F0}) dir:{_mvLastDetDir:F1} ndist:{_mvLastDetDist:F0} " +
@@ -882,7 +1011,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             // 屏幕分两行显示，便于直接观察（无需开启 DLog 磁盘转储）
             Hedingben.ToastText(
                 $"BASE vx{fleetVx:F3} fTh{fleetFrontTh:F1} | SEND vx{fleetVx:F3} c({cx:F0},{cy:F0},{cth:F1}) " +
-                $"| ready{fleetReady} canMove{canMove}",
+                $"| ready{fleetReady} canMove{canMove} stop{fleetStopActive}",
                 $"MultiVehicle{CarNum}-dbg");
             Hedingben.ToastText(
                 $"DET v{detectValid} dx{detectDx:F0} dy{detectDy:F0} dth{detectDth:F1} cmp({xDetectCompensate:F0},{yDetectCompensate:F0},{thDetectCompensate:F1}) " +
@@ -898,7 +1027,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                     layoutX, layoutY, layoutTh, selfAligned, ownDetectOk);
             }
 
-            if (fleetReady)
+            if (fleetReady || fleetStopActive)
             {
                 VehicleSyncNotification notification;
                 lock (FleetLock)
@@ -912,11 +1041,14 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                         CenterTh = CenterTh,
                         Aligned = MultiVehicleAligned,
                         Fleet = new Dictionary<int, VehicleSyncInfo>(MultiVehicleFleet),
-                        FleetVx = fleetVx,
-                        FleetFrontTh = fleetFrontTh,
-                        FleetRearTh = fleetRearTh,
+                        FleetVx = fleetStopActive ? 0 : fleetVx,
+                        FleetFrontTh = fleetStopActive ? 0 : fleetFrontTh,
+                        FleetRearTh = fleetStopActive ? 0 : fleetRearTh,
                         Mode = fleetMode,
-                        FleetOmega = fleetOmega,
+                        FleetOmega = fleetStopActive ? 0 : fleetOmega,
+                        FleetStopActive = fleetStopActive,
+                        FleetStopReason = fleetStopReason,
+                        FleetStopSourceCar = fleetStopSourceCar,
                         AutoEnabled = MultiVehicleAutoEnabled,
                         // 脚本驱动等价于手动联动，广播为 ManualEnabled 让从车解锁跟随。
                         ManualEnabled = manualEnabled,
@@ -1106,7 +1238,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             LayoutY = layoutY,
             LayoutTh = layoutTh,
             Aligned = aligned,
-            DetectOk = detectOk
+            DetectOk = detectOk,
+            MotionFeasible = _multiVehicleMotionFeasible,
+            MotionInfeasibleReason = _multiVehicleMotionInfeasibleReason
         };
     }
 

MultiWheel/MultiWheelC/VehicleSyncModels.cs

@@ -19,6 +19,8 @@ public class VehicleSyncInfo
     [JsonProperty("Aligned")] public bool Aligned { get; set; }
     // 本车本轮是否成功识别到邻车（关闭互识别时恒为 true）。任一车为 false 则整队停车。
     [JsonProperty("DetectOk")] public bool DetectOk { get; set; }
+    [JsonProperty("MotionFeasible")] public bool MotionFeasible { get; set; } = true;
+    [JsonProperty("MotionInfeasibleReason")] public string MotionInfeasibleReason { get; set; } = "";
 }
 
 public class VehicleSyncNotification
@@ -36,6 +38,9 @@ public class VehicleSyncNotification
     [JsonProperty("Mode")] public int Mode { get; set; }
     // 原地旋转角速度(deg/s，逆时针为正)，仅 Mode==2 有效
     [JsonProperty("FleetOmega")] public float FleetOmega { get; set; }
+    [JsonProperty("FleetStopActive")] public bool FleetStopActive { get; set; }
+    [JsonProperty("FleetStopReason")] public string FleetStopReason { get; set; } = "";
+    [JsonProperty("FleetStopSourceCar")] public int FleetStopSourceCar { get; set; }
     [JsonProperty("AutoEnabled")] public bool AutoEnabled { get; set; }
     [JsonProperty("ManualEnabled")] public bool ManualEnabled { get; set; }
     [JsonProperty("SyncTh")] public float SyncTh { get; set; }