Update MultiWheel fleet sync and restore clamp support

2026-06-30 22:20:10 +08:00
parent 3411654a34
commit 574987222a
14 changed files with 287 additions and 225 deletions
@@ -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))
@@ -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
    {
@@ -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;
@@ -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;
@@ -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
        };
    }

@@ -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; }
@@ -1,15 +0,0 @@
-{
-  "WheelConfig": {
-    "LeftFront": { "Position": { "X": 525.0, "Y": 200.0 } },
-    "LeftRear": { "Position": { "X": -525.0, "Y": 200.0 } },
-    "RightFront": { "Position": { "X": 525.0, "Y": -200.0 } },
-    "RightRear": { "Position": { "X": -525.0, "Y": -200.0 } }
-  },
-  "MaxSpeed": 0.3,
-  "AccPerSecond": 0.3,
-  "DeAccPerSecond": 0.5,
-  "MinTurnSpeedFac": 0.25,
-  "ControlPointRadius": 500.0,
-  "GcpThetaPerSecond": 10.0,
-  "MinimumTurningAngleForAckermann": 60.0
-}
@@ -1,60 +0,0 @@
-{
-  "basicSpeed": 0.2,
-  "DriveTaskInterval": 50,
-  "script": "MultiWheelC.dll",
-  "detourHost": "127.0.0.1",
-  "detourPort": 4321,
-  "msConf": {
-    "MultiVehicleMasterEndpoint": "/",
-    "MultiVehicleFleetNum": 2,
-    "MultiVehicleSyncInterval": 50,
-    "TestCarSyncDistance": 2400,
-    "TestCarSyncTh": 0,
-    "ManualCarSyncVxFac": 1.0,
-    "ManualCarSyncVthFac": 1.0,
-    "SyncThAccPerSec": 30,
-    "MultiVehicleSyncUseDetour": true,
-    "SimpleIp": "127.0.0.1",
-    "CarNum": 1
-  },
-  "layout": {
-    "chassis": {
-      "width": 1000,
-      "length": 1550,
-      "contour": [
-        750, -500, -750, -500, -750, -300, -800, -280, -800, -220,
-        -750, -200, -750, 200, -800, 220, -800, 280, -750, 300,
-        -750, 500, 750, 500
-      ]
-    },
-    "components": [
-      {
-        "type": "lidar2d",
-        "options": {
-          "name": "front_lidar_1",
-          "x": 700, "y": 0, "yaw": 0, "z": 0, "pitch": 0, "roll": 0,
-          "isCircle": false, "ignoreDist": 100, "maxDist": 200000,
-          "filterChassis": true
-        }
-      },
-      {
-        "type": "lidar2d",
-        "options": {
-          "name": "rear_left_lidar_1",
-          "x": -700, "y": 450, "yaw": 180, "z": 0, "pitch": 0, "roll": 0,
-          "isCircle": false, "ignoreDist": 100, "maxDist": 200000,
-          "filterChassis": true
-        }
-      },
-      {
-        "type": "lidar2d",
-        "options": {
-          "name": "rear_right_lidar_1",
-          "x": -700, "y": -450, "yaw": 180, "z": 0, "pitch": 0, "roll": 0,
-          "isCircle": false, "ignoreDist": 100, "maxDist": 200000,
-          "filterChassis": true
-        }
-      }
-    ]
-  }
-}
@@ -1,11 +0,0 @@
-{
-  "port": 8008,
-  "allowMultiple": true,
-  "soTag": "Multi1",
-  "detourHost": "127.0.0.1",
-  "detourPort": 4321,
-  "HideConsoleOnStart": false,
-  "FollowCarOnStart": true,
-  "ShowRobot3dModel": true,
-  "ShowRobotArrow": true
-}
@@ -1,60 +0,0 @@
-{
-  "basicSpeed": 0.2,
-  "DriveTaskInterval": 50,
-  "script": "MultiWheelC.dll",
-  "detourHost": "127.0.0.1",
-  "detourPort": 4421,
-  "msConf": {
-    "MultiVehicleMasterEndpoint": "127.0.0.1:8008",
-    "MultiVehicleFleetNum": 2,
-    "MultiVehicleSyncInterval": 50,
-    "TestCarSyncDistance": 2400,
-    "TestCarSyncTh": 0,
-    "ManualCarSyncVxFac": 1.0,
-    "ManualCarSyncVthFac": 1.0,
-    "SyncThAccPerSec": 30,
-    "MultiVehicleSyncUseDetour": true,
-    "SimpleIp": "127.0.0.1",
-    "CarNum": 2
-  },
-  "layout": {
-    "chassis": {
-      "width": 1000,
-      "length": 1550,
-      "contour": [
-        750, -500, -750, -500, -750, -300, -800, -280, -800, -220,
-        -750, -200, -750, 200, -800, 220, -800, 280, -750, 300,
-        -750, 500, 750, 500
-      ]
-    },
-    "components": [
-      {
-        "type": "lidar2d",
-        "options": {
-          "name": "front_lidar_2",
-          "x": 700, "y": 0, "yaw": 0, "z": 0, "pitch": 0, "roll": 0,
-          "isCircle": false, "ignoreDist": 100, "maxDist": 200000,
-          "filterChassis": true
-        }
-      },
-      {
-        "type": "lidar2d",
-        "options": {
-          "name": "rear_left_lidar_2",
-          "x": -700, "y": 450, "yaw": 180, "z": 0, "pitch": 0, "roll": 0,
-          "isCircle": false, "ignoreDist": 100, "maxDist": 200000,
-          "filterChassis": true
-        }
-      },
-      {
-        "type": "lidar2d",
-        "options": {
-          "name": "rear_right_lidar_2",
-          "x": -700, "y": -450, "yaw": 180, "z": 0, "pitch": 0, "roll": 0,
-          "isCircle": false, "ignoreDist": 100, "maxDist": 200000,
-          "filterChassis": true
-        }
-      }
-    ]
-  }
-}
@@ -1,11 +0,0 @@
-{
-  "port": 8009,
-  "allowMultiple": true,
-  "soTag": "Multi2",
-  "detourHost": "127.0.0.1",
-  "detourPort": 4421,
-  "HideConsoleOnStart": false,
-  "FollowCarOnStart": true,
-  "ShowRobot3dModel": true,
-  "ShowRobotArrow": true
-}
@@ -1,17 +0,0 @@
-{
-  "basicSpeed": 0.2,
-  "DriveTaskInterval": 50,
-  "script": "DiffWheelC.dll",
-  "msConf": {
-    "ChassisWidth": 600,
-    "SimpleIp": "127.0.0.1"
-  },
-  "layout": {
-    "chassis": {
-      "width": 600,
-      "length": 900,
-      "contour": [-450, 300, 450, 300, 450, -300, -450, -300]
-    },
-    "components": []
-  }
-}
@@ -1,9 +0,0 @@
-@echo off
-setlocal
-cd /d "%~dp0"
-if not exist chassis.json copy /Y ..\..\deploy\chassis.json chassis.json >nul
-if not exist clumsy.json copy /Y ..\..\deploy\clumsy_agv1\clumsy.json clumsy.json >nul
-if not exist clumsyconsole.json copy /Y ..\..\deploy\clumsy_agv1\clumsyconsole.json clumsyconsole.json >nul
-echo [Clumsy AGV1] Master, tag Multi1. Start Medulla build\Medulla first.
-ClumsyLite.exe
-pause
@@ -1,9 +0,0 @@
-@echo off
-setlocal
-cd /d "%~dp0"
-if not exist chassis.json copy /Y ..\..\deploy\chassis.json chassis.json >nul
-if not exist clumsy.json copy /Y ..\..\deploy\clumsy_agv2\clumsy.json clumsy.json >nul
-if not exist clumsyconsole.json copy /Y ..\..\deploy\clumsy_agv2\clumsyconsole.json clumsyconsole.json >nul
-echo [Clumsy AGV2] Slave, tag Multi2. Start Medulla build\Medulla_AGV2 first.
-ClumsyLite.exe
-pause