Fix fleet in-place rotation coordination

- Gate rotate-mode compensation PI by active fleet omega and reset the integrator/timer when idle, so releasing the joystick stops the cars cleanly instead of drifting/self-rotating from a wound-up integral. - Add conditional integration anti-windup in RotatePiTerm to reduce the startup overshoot after the initial relative-pose spike. - Raise translational P gain for faster startup correction. - Superimpose body-frame compensation onto swerve rotation and add MultiVehicleDbg / RotateDbg / RotatePoseDbg diagnostics. Co-authored-by: Cursor <cursoragent@cursor.com>
2026-06-28 19:09:24 +08:00
parent 235c6c70ac
commit d3abf0fb02
4 changed files with 387 additions and 112 deletions
@@ -12,6 +12,7 @@ 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;
    [FieldMember(desc = "[sync] 手动Vth系数")] public float ManualCarSyncVthFac = 1f;
    [FieldMember(desc = "[sync] 检测中心偏移(mm)")] public float DeltaDetectCenter = 350f;
    [FieldMember(desc = "[sync] 是否启用Detour定位")] public bool PosAvailable = true;
@@ -47,6 +48,20 @@ public class PilotConfig : MultiWheelPilotConfig
    [FieldMember(desc = "多车联动：互识别 Y补偿阈值(mm)")] public float MultiVehicleDetectBiasYThreshold = 50f;
    [FieldMember(desc = "多车联动：互识别 Th补偿阈值(deg)")] public float MultiVehicleDetectBiasThThreshold = 5f;

+    // 原地旋转(mode2)闭环纠偏(PI)：把"本车应移动到的位置(dx,dy,mm)/应转角(dth,deg)"作为误差，
+    // 用 PI 控制器换算成车体系修正速度叠加到绕队心旋转上。纯 P 对抗恒定横向滑移扰动有稳态残差，
+    // 加积分项把稳态误差拉到 0；积分带限幅(抗 windup)，总输出限幅在 Max 内防过冲/振荡。
+    // Fac=比例增益(mm/s per mm、deg/s per deg)；IFac=积分增益(mm/s per mm·s、deg/s per deg·s)；Max=总输出上限。
+    [FieldMember(desc = "原地旋转纠偏：平移比例增益P(mm/s per mm)")] public float MultiVehicleRotateCompXyFac = 1.2f;
+    [FieldMember(desc = "原地旋转纠偏：平移积分增益I(mm/s per mm·s)")] public float MultiVehicleRotateCompXyIFac = 0.8f;
+    [FieldMember(desc = "原地旋转纠偏：平移速度上限(mm/s)")] public float MultiVehicleRotateCompXyMax = 150f;
+    [FieldMember(desc = "原地旋转纠偏：转向比例增益P(deg/s per deg)")] public float MultiVehicleRotateCompThFac = 0.8f;
+    [FieldMember(desc = "原地旋转纠偏：转向积分增益I(deg/s per deg·s)")] public float MultiVehicleRotateCompThIFac = 0.8f;
+    [FieldMember(desc = "原地旋转纠偏：转向速度上限(deg/s)")] public float MultiVehicleRotateCompThMax = 15f;
+    // 仅当车队实际被指令旋转(|fleetOmega|超过此阈值)时才运行纠偏 PI；否则清零并复位积分，
+    // 避免松开摇杆后积分残留持续驱动车辆"自行旋转停不下来"。
+    [FieldMember(desc = "原地旋转纠偏：生效的最小角速度阈值(deg/s)")] public float MultiVehicleRotateActiveOmega = 0.5f;
+
    [FieldMember(desc = "单车同步 xy 精度(mm)")] public float SingleCarSyncPrecisionXy = 10f;
    [FieldMember(desc = "单车同步 th 精度(deg)")] public float SingleCarSyncPrecisionTh = 0.2f;

@@ -56,6 +71,9 @@ public class PilotConfig : MultiWheelPilotConfig
    [FieldMember(desc = "Playground 小车名称（场景 robots[].name）")]
    public string PlaygroundRobotName = "agv_multi_1";

+    [FieldMember(desc = "Playground 邻车名称（仅主车用于原地旋转位姿诊断）")]
+    public string PlaygroundNeighborRobotName = "agv_multi_2";
+
    [FieldMember(desc = "WebAPI 平移测试：平移距离(mm)")]
    public float WebApiTranslateMm = 100f;

@@ -64,30 +64,25 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
    // 诊断日志：节流计时 + 最近一次检测几何（中心/朝向/距离），用于定位剧烈运动来源
    private DateTime _mvDbgLastLog = DateTime.MinValue;
    private float _mvLastDetCenterX, _mvLastDetCenterY, _mvLastDetDir, _mvLastDetDist;
+    // 原地旋转(mode2)实际叠加的车体系纠偏旋量(mm/s, mm/s, deg/s)，仅用于诊断日志。
+    private float _mvRotCompVx, _mvRotCompVy, _mvRotCompOmega;
+    // 原地旋转纠偏 PI 控制器的积分累加器(mm·s, mm·s, deg·s)与上次计算时刻。
+    private float _rotIntegX, _rotIntegY, _rotIntegTh;
+    private DateTime _rotPiLastTime = DateTime.MinValue;

-    // 写盘诊断日志（Clumsy 侧）。文件落在 Clumsy 工作目录，复现后离线分析。
-    // 每次进程启动清空一次（_fleetDiagInit），同一次运行内追加，避免跨多次运行累积。
+    // 原地旋转“两车实际 sim 位姿”诊断采样状态（仅主车，通过 Playground WebAPI 读取）。
+    private DateTime _rotPoseLastLog = DateTime.MinValue;
+    private DateTime _rotPosePrevTime = DateTime.MinValue;
+    private float _rotPosePrevYaw1, _rotPosePrevYaw2;
+    private bool _rotPoseEpisode;          // 本次旋转片段是否已捕获参考量
+    private float _rotPoseMid0X, _rotPoseMid0Y; // 起转瞬间车队几何中心(世界系)，用于度量整体平移漂移
+
+    // 写盘诊断日志（Clumsy 侧）。改用 DLog 落盘：同一 topic 的日志会归到以 topic 命名的文件夹，
+    // 由 DLog 统一管理落点/滚动，无需自行维护文件句柄与 session 清空逻辑。
    private DateTime _mvDiskLastLog = DateTime.MinValue;
-    private static bool _fleetDiagInit;
    private void FleetDiag(string msg)
    {
-        try
-        {
-            const string path = "fleet_diag_clumsy.log";
-            var line = $"{DateTime.Now:HH:mm:ss.fff} car{CarNum} {msg}{Environment.NewLine}";
-            if (!_fleetDiagInit)
-            {
-                _fleetDiagInit = true;
-                System.IO.File.WriteAllText(path,
-                    $"=== session start {DateTime.Now:yyyy-MM-dd HH:mm:ss} ==={Environment.NewLine}" + line);
-            }
-            else
-                System.IO.File.AppendAllText(path, line);
-        }
-        catch
-        {
-            // 诊断日志失败不影响主流程
-        }
+        DLog.Log($"car{CarNum} {msg}", "FleetDiagClumsy");
    }

    // 互识别：邻车两腿检测的滑动窗口（最近 1s，最多 10 帧），用于平滑抖动
@@ -222,6 +217,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            }
        });

+        DLog.Log("MultiVehicle coordination initialized, starting loop thread", "MultiVehicle");
+        Console.WriteLine("[MultiVehicle] coordination initialized, starting loop thread");
        new Thread(() => MultiVehicleLoop(hc)) { Name = "MultiVehicle", IsBackground = true }.Start();
    }

@@ -246,6 +243,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            Clear = () => sendMotionPainter.Clear()
        };

+        DLog.Log("MultiVehicleLoop thread running", "MultiVehicle");
+        Console.WriteLine("[MultiVehicle] loop thread running");
+        var loopCount = 0L;
        while (true)
        {
            try
@@ -257,6 +257,10 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                DLog.Log($"MultiVehicle loop error: {e.FormatEx()}", "MultiVehicle");
            }

+            // 每 ~5s 打一条心跳，确认循环确实在跑（用于区分“循环没跑”与“写盘失败”）
+            if (loopCount++ % Math.Max(1, 5000 / Math.Max(1, Conf.MultiVehicleSyncInterval)) == 0)
+                DLog.Log($"MultiVehicleLoop heartbeat #{loopCount}", "MultiVehicle");
+
            Thread.Sleep(Conf.MultiVehicleSyncInterval);
        }
    }
@@ -338,7 +342,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
        var syncTh = Conf.TestCarSyncTh;
        var syncDistance = Conf.TestCarSyncDistance;
        var deltaDetectCenter = Conf.DeltaDetectCenter;
-        float fleetVx = 0, fleetFrontTh = 0, fleetRearTh = 0;
+        float fleetVx = 0, fleetFrontTh = 0, fleetRearTh = 0, fleetOmega = 0;
+        var fleetMode = 0; // 0=常规 1=蟹行 2=原地旋转

        CenterX = CenterY = CenterTh = 0;

@@ -347,6 +352,35 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            if (MultiVehicleManualEnabled)
            {
                syncTh = 0;
+                fleetMode = MultiVehicleManualMode;
+                if (fleetMode == 2)
+                {
+                    // 原地旋转：摇杆左右 → 绕车队中心角速度(deg/s)。底盘 SetOriginBias 已设为车队中心。
+                    fleetOmega = MultiVehicleManualVth * Conf.ManualCarSyncVthFac;
+                    fleetVx = 0;
+                    fleetFrontTh = 0;
+                    fleetRearTh = 0;
+                    _multiVehicleAccumulateTh = 0f;
+                    _multiVehicleLastThTime = DateTime.Now;
+                }
+                else if (fleetMode == 1)
+                {
+                    // 蟹行：把 (前后向Vx, 横向Vy) 合成速度矢量，四轮同向打到该方向（前后舵轮角相同）。
+                    // 舵轮角限制在 ±90°，超出则取反向并令速度取负，避免出现 180° 这类不可达转角。
+                    var vx = MultiVehicleManualVx * Conf.ManualCarSyncVxFac;
+                    var vy = MultiVehicleManualVy * Conf.ManualCarSyncVyFac;
+                    var speed = (float)Math.Sqrt(vx * vx + vy * vy);
+                    var crabAngle = (float)(Math.Atan2(vy, vx) * 180.0 / Math.PI);
+                    if (crabAngle > 90f) { crabAngle -= 180f; speed = -speed; }
+                    else if (crabAngle < -90f) { crabAngle += 180f; speed = -speed; }
+                    fleetVx = speed;
+                    fleetFrontTh = crabAngle;
+                    fleetRearTh = crabAngle;
+                    _multiVehicleAccumulateTh = 0f;
+                    _multiVehicleLastThTime = DateTime.Now;
+                }
+                else
+                {
                    fleetVx = MultiVehicleManualVx * Conf.ManualCarSyncVxFac;
                    var targetTh = MultiVehicleManualVth * Conf.ManualCarSyncVthFac;
                    var now = DateTime.Now;
@@ -358,6 +392,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                    fleetFrontTh = _multiVehicleAccumulateTh;
                    fleetRearTh = -fleetFrontTh;
                }
+            }
            else
            {
                fleetVx = MultiVehicleAutoVx;
@@ -382,6 +417,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            fleetVx = notification.FleetVx;
            fleetFrontTh = notification.FleetFrontTh;
            fleetRearTh = notification.FleetRearTh;
+            fleetMode = notification.Mode;
+            fleetOmega = notification.FleetOmega;
        }

        var (layoutX, layoutY, layoutTh) = GetLayoutPose(syncTh, syncDistance);
@@ -451,6 +488,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            fleetVx = 0;
            fleetFrontTh = 0;
            fleetRearTh = 0;
+            fleetOmega = 0;
            Hedingben.ToastText(
                $"车队停车：{(!ownDetectOk ? "本车" : "其它车")}2腿检测丢失（速度已置零）",
                $"MultiVehicle{CarNum}-stop");
@@ -516,11 +554,71 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            }

            sendMotionPainter.Clear();
+            if (fleetMode == 2)
+            {
+                // 原地旋转：底盘原点已偏置到车队中心，绕队中心旋转，并叠加 PI 闭环纠偏维持队形。
+                // detect/pos 偏差(dx,dy,dth) 本身就是"本车参考点应在车体系移动到的位置/应转的角"，作为误差喂给 PI。
+                // 纯 P 对抗恒定横向滑移有稳态残差，积分项消除之。检测丢失(canMove=false)时不纠偏并清空积分。
+                float rotCompVx = 0, rotCompVy = 0, rotCompOmega = 0;
+                var now = DateTime.Now;
+                // dt 限幅，避免首帧/卡顿导致积分突跳
+                var dt = _rotPiLastTime == DateTime.MinValue ? 0f
+                    : (float)Math.Min(0.2, Math.Max(0, (now - _rotPiLastTime).TotalSeconds));
+                _rotPiLastTime = now;
+
+                // 仅在"被指令旋转"时才纠偏：松开摇杆(fleetOmega≈0)时绝不再下发补偿，
+                // 否则积分残留会持续驱动车辆平移/旋转，表现为"松杆后轮子来回打、自转停不下来"。
+                var rotating = Math.Abs(fleetOmega) > Conf.MultiVehicleRotateActiveOmega;
+                if (canMove && rotating)
+                {
+                    var errX = detectDx + posBiasX;    // mm，车体系：本车纵向(前+)应移动量
+                    var errY = detectDy + posBiasY;    // mm，车体系：本车横向(左+)应移动量
+                    var errTh = detectDth + posBiasTh; // deg，本车应转角
+                    rotCompVx = RotatePiTerm(errX, ref _rotIntegX, Conf.SingleCarSyncPrecisionXy,
+                        Conf.MultiVehicleRotateCompXyFac, Conf.MultiVehicleRotateCompXyIFac,
+                        Conf.MultiVehicleRotateCompXyMax, dt);
+                    rotCompVy = RotatePiTerm(errY, ref _rotIntegY, Conf.SingleCarSyncPrecisionXy,
+                        Conf.MultiVehicleRotateCompXyFac, Conf.MultiVehicleRotateCompXyIFac,
+                        Conf.MultiVehicleRotateCompXyMax, dt);
+                    rotCompOmega = RotatePiTerm(errTh, ref _rotIntegTh, Conf.SingleCarSyncPrecisionTh,
+                        Conf.MultiVehicleRotateCompThFac, Conf.MultiVehicleRotateCompThIFac,
+                        Conf.MultiVehicleRotateCompThMax, dt);
+                }
+                else
+                {
+                    // 检测丢失或未指令旋转：清零补偿并复位积分/计时，停止时不再有残留驱动。
+                    _rotIntegX = _rotIntegY = _rotIntegTh = 0;
+                    _rotPiLastTime = DateTime.MinValue;
+                }
+
+                _mvRotCompVx = rotCompVx;
+                _mvRotCompVy = rotCompVy;
+                _mvRotCompOmega = rotCompOmega;
+
+                chassis.SendRotateMotion(fleetOmega,
+                    localCompensateX: rotCompVx, localCompensateY: rotCompVy, localCompensateTh: rotCompOmega);
+
+                // 仅主车：读取两车实际 sim 位姿，量化"开环横向滑移"来源（节流 ~200ms）。
+                if (isMaster)
+                    LogRotatePoseSample();
+            }
+            else
+            {
+                _mvRotCompVx = 0;
+                _mvRotCompVy = 0;
+                _mvRotCompOmega = 0;
+                // 退出原地旋转：清空 PI 积分与计时、位姿诊断片段，下次进入重新起算。
+                _rotIntegX = _rotIntegY = _rotIntegTh = 0;
+                _rotPiLastTime = DateTime.MinValue;
+                _rotPoseEpisode = false;
+                _rotPosePrevTime = DateTime.MinValue;
+                // 常规/蟹行：蟹行时 frontTh==rearTh（四轮同向）即为平移，与常规共用同一下发路径。
                chassis.SendMotion(fleetVx, fleetFrontTh, fleetRearTh, localControlRadius: 510,
                    localCompensateX: xDetectCompensate + xPosCompensate,
                    localCompensateY: yDetectCompensate + yPosCompensate,
                    localCompensateTh: thDetectCompensate + thPosCompensate);
            }
+        }

        // === 诊断日志（节流 ~300ms）===
        // 用于定位“剧烈运动”来源：BASE=遥控基础速度；DETECT=2腿检测补偿；POS=SLAM编队补偿；SEND=最终下发。
@@ -544,7 +642,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                $"| POS self({selfX:F0},{selfY:F0},{selfTh:F1}) center({CenterX:F0},{CenterY:F0},{CenterTh:F1}) " +
                $"bias({posBiasX:F0},{posBiasY:F0},{posBiasTh:F1}) -> comp x:{xPosCompensate:F1} y:{yPosCompensate:F1} th:{thPosCompensate:F2} " +
                $"| LAYOUT({layoutX:F0},{layoutY:F0},{layoutTh:F0}) R:{syncDistance / 2f:F0} " +
-                $"| SEND vx:{fleetVx:F3} fTh:{fleetFrontTh:F2} rTh:{fleetRearTh:F2} cx:{cx:F1} cy:{cy:F1} cth:{cth:F2}";
+                $"| SEND mode:{fleetMode} omega:{fleetOmega:F1} vx:{fleetVx:F3} fTh:{fleetFrontTh:F2} rTh:{fleetRearTh:F2} cx:{cx:F1} cy:{cy:F1} cth:{cth:F2} " +
+                $"rotComp(vx:{_mvRotCompVx:F1} vy:{_mvRotCompVy:F1} om:{_mvRotCompOmega:F2})";

            DLog.Log(dbg, "MultiVehicleDbg");
            FleetDiag(dbg);
@@ -584,6 +683,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                        FleetVx = fleetVx,
                        FleetFrontTh = fleetFrontTh,
                        FleetRearTh = fleetRearTh,
+                        Mode = fleetMode,
+                        FleetOmega = fleetOmega,
                        AutoEnabled = MultiVehicleAutoEnabled,
                        ManualEnabled = MultiVehicleManualEnabled,
                        SyncTh = syncTh,
@@ -769,5 +870,112 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
    private static float ClampBias(float value, float threshold)
        => Math.Sign(value) * Math.Min(Math.Abs(value), threshold);

+    /// <summary>
+    /// 原地旋转纠偏单轴 PI 控制器。err 为车体系偏差(mm 或 deg)，输出为修正速度(mm/s 或 deg/s)，
+    /// 带死区、积分抗饱和(限制积分贡献在 ±max 内)与总输出限幅。死区内冻结积分(保留稳态修正以抵消恒定扰动)。
+    /// </summary>
+    private static float RotatePiTerm(float err, ref float integ, float deadband,
+        float pFac, float iFac, float max, float dt)
+    {
+        // 死区内：不再累加误差，仅输出已积累的积分项(维持对恒定扰动的稳态补偿)。
+        if (Math.Abs(err) < deadband)
+            return ClampBias(integ * iFac, max);
+
+        // 条件积分(抗 windup)：仅当总输出未在同向饱和时才累加误差。
+        // 起步阶段大误差会让 P 项接近/超过 max，此时继续积分会顶满积分器，
+        // 误差反向后需很久才能泄放，造成纠偏"过冲再回拉"。此处饱和即停积分。
+        var unclamped = err * pFac + integ * iFac;
+        var saturatedSameSign = Math.Abs(unclamped) >= max && Math.Sign(unclamped) == Math.Sign(err);
+        if (!saturatedSameSign)
+            integ += err * dt;
+
+        var iTerm = integ * iFac;
+        // 抗积分饱和：把积分贡献限制在 ±max，并反算回写积分器，避免 windup。
+        if (iFac > 1e-9f)
+        {
+            var iLimit = max / iFac;
+            if (integ > iLimit) integ = iLimit;
+            else if (integ < -iLimit) integ = -iLimit;
+            iTerm = integ * iFac;
+        }
+
+        return ClampBias(err * pFac + iTerm, max);
+    }
+
+    /// <summary>
+    /// 主车专用：通过 Playground WebAPI 读取本车与邻车的真实 sim 位姿，落 RotatePoseDbg 日志。
+    /// 量化"开环横向滑移"来源：
+    ///   - w1/w2/dw：两车实际偏航角速率(deg/s)。dw 持续非零 ⇒ 主从转速不一致(相对旋转)。
+    ///   - rel_in1(x,y,dyaw)：邻车在本车体系下的真实相对位姿；dyaw=实际相对朝向−180°。
+    ///     dyaw≈0 但 y 持续漂 ⇒ 纯横向平移滑移(非角度滞后，印证 B 无用)。
+    ///   - midDrift：车队几何中心(世界系)相对起转瞬间的位移 ⇒ 整体平移漂移量。
+    /// 节流 ~200ms；WebAPI 异常时静默跳过，不影响控制环。
+    /// </summary>
+    private void LogRotatePoseSample()
+    {
+        var now = DateTime.Now;
+        if ((now - _rotPoseLastLog).TotalMilliseconds < 200) return;
+        _rotPoseLastLog = now;
+
+        var neighbor = Conf.PlaygroundNeighborRobotName;
+        if (string.IsNullOrWhiteSpace(neighbor) || neighbor == Conf.PlaygroundRobotName) return;
+
+        try
+        {
+            var p1 = PlaygroundWebApi.GetPose(Conf.PlaygroundWebApiUrl, Conf.PlaygroundRobotName);
+            var p2 = PlaygroundWebApi.GetPose(Conf.PlaygroundWebApiUrl, neighbor);
+
+            // 邻车在本车(car1)体系下的相对位姿：rel = R(-yaw1)·(p2-p1)
+            var ddx = p2.X - p1.X;
+            var ddy = p2.Y - p1.Y;
+            var th1 = p1.YawDeg * (float)Math.PI / 180f;
+            var c = (float)Math.Cos(th1);
+            var s = (float)Math.Sin(th1);
+            var relX = ddx * c + ddy * s;
+            var relY = -ddx * s + ddy * c;
+            var relYaw = (float)LessMath.ThDiff(p2.YawDeg - p1.YawDeg, 180f); // 实际相对朝向偏离 180° 的量
+            var dist = (float)Math.Sqrt(ddx * ddx + ddy * ddy);
+
+            var midX = (p1.X + p2.X) / 2f;
+            var midY = (p1.Y + p2.Y) / 2f;
+            if (!_rotPoseEpisode)
+            {
+                _rotPoseEpisode = true;
+                _rotPoseMid0X = midX;
+                _rotPoseMid0Y = midY;
+            }
+            var midDx = midX - _rotPoseMid0X;
+            var midDy = midY - _rotPoseMid0Y;
+
+            // 实际偏航角速率（数值微分）
+            float w1 = 0, w2 = 0, dw = 0;
+            if (_rotPosePrevTime != DateTime.MinValue)
+            {
+                var dt = (float)(now - _rotPosePrevTime).TotalSeconds;
+                if (dt > 1e-3)
+                {
+                    w1 = (float)LessMath.ThDiff(p1.YawDeg, _rotPosePrevYaw1) / dt;
+                    w2 = (float)LessMath.ThDiff(p2.YawDeg, _rotPosePrevYaw2) / dt;
+                    dw = w1 - w2;
+                }
+            }
+            _rotPosePrevTime = now;
+            _rotPosePrevYaw1 = p1.YawDeg;
+            _rotPosePrevYaw2 = p2.YawDeg;
+
+            DLog.Log(
+                $"car1({p1.X:F0},{p1.Y:F0},{p1.YawDeg:F2}) car2({p2.X:F0},{p2.Y:F0},{p2.YawDeg:F2}) " +
+                $"| rel_in1(x:{relX:F0} y:{relY:F0} dyaw:{relYaw:F2}) dist:{dist:F0} " +
+                $"| mid({midX:F0},{midY:F0}) midDrift(dx:{midDx:F0} dy:{midDy:F0} |d|:{Math.Sqrt(midDx * midDx + midDy * midDy):F0}) " +
+                $"| w1:{w1:F2} w2:{w2:F2} dw:{dw:F2}",
+                "RotatePoseDbg");
+        }
+        catch (Exception e)
+        {
+            // 诊断用途，WebAPI 不通时静默（仅偶发提示），不打断控制环。
+            Hedingben.ToastText($"RotatePose WebAPI err: {e.Message}", $"MultiVehicle{CarNum}-rotpose");
+        }
+    }
+
    #endregion
 }
@@ -32,6 +32,10 @@ public class VehicleSyncNotification
    [JsonProperty("FleetVx")] public float FleetVx { get; set; }
    [JsonProperty("FleetFrontTh")] public float FleetFrontTh { get; set; }
    [JsonProperty("FleetRearTh")] public float FleetRearTh { get; set; }
+    // 联动运动模式：0=常规(前进+转向) 1=蟹行(四轮同向平移) 2=原地旋转(绕车队中心)
+    [JsonProperty("Mode")] public int Mode { get; set; }
+    // 原地旋转角速度(deg/s，逆时针为正)，仅 Mode==2 有效
+    [JsonProperty("FleetOmega")] public float FleetOmega { get; set; }
    [JsonProperty("AutoEnabled")] public bool AutoEnabled { get; set; }
    [JsonProperty("ManualEnabled")] public bool ManualEnabled { get; set; }
    [JsonProperty("SyncTh")] public float SyncTh { get; set; }
@@ -1,8 +1,8 @@
 using System;
-using System.IO;
 using CartActivator;
 using CycleGUI;
 using CycleGUI.API;
+using FundamentalLib;
 using Medulla.Types;

 namespace MultiWheelM;
@@ -12,28 +12,11 @@ public partial class CartDefinition
    private bool _fleetRemoteActive;
    private DateTime _fleetDiagLastStick = DateTime.MinValue;

-    // 写盘诊断日志（Medulla 侧）。文件落在 Medulla 工作目录，便于复现后离线分析。
-    // 每次进程启动清空一次（_fleetDiagInit），同一次运行内追加，避免跨多次运行累积。
-    private static bool _fleetDiagInit;
+    // 写盘诊断日志（Medulla 侧）。改用 DLog 落盘：同一 topic 的日志会归到以 topic 命名的文件夹，
+    // 由 DLog 统一管理落点/滚动，无需自行维护文件句柄与 session 清空逻辑。
    private void FleetDiag(string msg)
    {
-        try
-        {
-            const string path = "fleet_diag_medulla.log";
-            var line = $"{DateTime.Now:HH:mm:ss.fff} car{CarNum} {msg}{Environment.NewLine}";
-            if (!_fleetDiagInit)
-            {
-                _fleetDiagInit = true;
-                File.WriteAllText(path,
-                    $"=== session start {DateTime.Now:yyyy-MM-dd HH:mm:ss} ==={Environment.NewLine}" + line);
-            }
-            else
-                File.AppendAllText(path, line);
-        }
-        catch
-        {
-            // 诊断日志失败不影响主流程
-        }
+        DLog.Log($"car{CarNum} {msg}", "FleetDiagMedulla");
    }

    private void ZeroFleetManualFields()
@@ -56,69 +39,36 @@ public partial class CartDefinition

        _fleetRemoteActive = true;
        var fleetOn = false;
-        // 速度比例 0~1，作用于摇杆输出的线速度与转向角。
+        // 运动模式开关：互斥由用户操作保证，代码用 if/else 优先级确保同一时刻只有一个模式生效。
+        var crabOn = false;   // 蟹行（四轮同向平移）
+        var rotateOn = false; // 原地旋转（绕车队中心）
+        // 速度比例 0~1，作用于摇杆输出的线速度/横移/角速度。
        var speedRatio = 1f;
        UseGesture? manip = null;

+        // 0=常规(前进+转向) 1=蟹行 2=原地旋转。crab 优先于 rotate（同时打开时蟹行生效）。
+        int CurrentMode() => crabOn ? 1 : rotateOn ? 2 : 0;
+        void ApplyMode() => MultiVehicleManualMode = fleetOn ? CurrentMode() : 0;
+
        void CleanupFleetRemote()
        {
            manip?.End();
            manip = null;
            fleetOn = false;
+            crabOn = false;
+            rotateOn = false;
            ZeroFleetManualFields();
            _fleetRemoteActive = false;
        }

        manip = new UseGesture();

-        manip.AddWidget(new UseGesture.ToggleWidget
-        {
-            name = "fleet_enable",
-            text = "车队联动",
-            position = "12.5%+10px, 75%+10px",
-            size = "25%-10px, 12.5%-10px",
-            OnValue = b =>
-            {
-                fleetOn = b;
-                MultiVehicleManualEnabled = b;
-                MultiVehicleManualMode = 0;
-                if (!b)
-                    ZeroFleetManualFields();
-                FleetDiag($"TOGGLE fleetOn={b} -> ManualEnabled={MultiVehicleManualEnabled}");
-            }
-        });
-
-        manip.AddWidget(new UseGesture.ThrottleWidget
-        {
-            name = "fleet_speed_ratio",
-            text = "速度比例",
-            position = "50%+10px, 75%+10px",
-            size = "50%-10px, 12.5%-10px",
-            bounceBack = false,
-            OnValue = (val, _) => speedRatio = Math.Clamp(val, 0, 1)
-        });
-
-        manip.AddWidget(new UseGesture.ButtonWidget
-        {
-            name = "fleet_stop",
-            text = "急停/归零",
-            position = "25%+10px, 87.5%+10px",
-            size = "25%-10px, 12.5%-10px",
-            OnPressed = _ =>
-            {
-                MultiVehicleManualVx = 0;
-                MultiVehicleManualVy = 0;
-                MultiVehicleManualVth = 0;
-                FleetDiag("BUTTON stop/zero");
-            }
-        });
-
        manip.AddWidget(new UseGesture.StickWidget
        {
            name = "fleet_stick",
            text = "速度摇杆",
-            position = "37.5%+10px, 25%+10px",
-            size = "37.5%-10px, 37.5%-10px",
+            position = "37.5%+10px, 18%+10px",
+            size = "37.5%-10px, 32%-10px",
            bounceBack = true,
            keyboard = "Up,Down,Left,Right",
            joystick = "Axis0,Axis1",
@@ -137,22 +87,116 @@ public partial class CartDefinition
                    return;
                }

-                // pos.Y(前后) → 车队线速度(m/s)；pos.X(左右) → 车队转向角(deg)。松手 bounceBack 自动回零。
                var ratio = Math.Clamp(speedRatio, 0, 1);
-                MultiVehicleManualVx = Math.Clamp(pos.Y, -1, 1) * MaxManualSpeed * ratio;
+                var px = Math.Clamp(pos.X, -1, 1);
+                var py = Math.Clamp(pos.Y, -1, 1);
+                var mode = CurrentMode();
+                MultiVehicleManualMode = mode;
+
+                if (mode == 2)
+                {
+                    // 原地旋转：pos.X(左右) → 角速度(deg/s)，绕车队中心。
+                    MultiVehicleManualVx = 0;
                    MultiVehicleManualVy = 0;
-                MultiVehicleManualVth = Math.Clamp(pos.X, -1, 1) * MaxManualAngularSpeed * ratio;
+                    MultiVehicleManualVth = px * MaxManualAngularSpeed * ratio;
+                }
+                else if (mode == 1)
+                {
+                    // 蟹行：pos.Y → 前后向线速度，pos.X → 横向线速度（m/s）。
+                    MultiVehicleManualVx = py * MaxManualSpeed * ratio;
+                    MultiVehicleManualVy = px * MaxManualSpeed * ratio;
+                    MultiVehicleManualVth = 0;
+                }
+                else
+                {
+                    // 常规：pos.Y → 线速度(m/s)，pos.X → 转向角(deg)。
+                    MultiVehicleManualVx = py * MaxManualSpeed * ratio;
+                    MultiVehicleManualVy = 0;
+                    MultiVehicleManualVth = px * MaxManualAngularSpeed * ratio;
+                }

                if ((DateTime.Now - _fleetDiagLastStick).TotalMilliseconds >= 200)
                {
                    _fleetDiagLastStick = DateTime.Now;
-                    FleetDiag($"STICK pos=({pos.X:0.00},{pos.Y:0.00}) manip={manipulating} ratio={ratio:0.00} " +
-                              $"maxV={MaxManualSpeed:0.00} maxW={MaxManualAngularSpeed:0.0} " +
-                              $"-> Vx={MultiVehicleManualVx:0.000} Vth={MultiVehicleManualVth:0.0} en={MultiVehicleManualEnabled}");
+                    FleetDiag($"STICK mode={mode} pos=({pos.X:0.00},{pos.Y:0.00}) manip={manipulating} ratio={ratio:0.00} " +
+                              $"-> Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.0} en={MultiVehicleManualEnabled}");
                }
            }
        });

+        manip.AddWidget(new UseGesture.ToggleWidget
+        {
+            name = "fleet_crab",
+            text = "蟹行模式",
+            position = "12.5%+10px, 52%+10px",
+            size = "37.5%-10px, 10%-10px",
+            OnValue = b =>
+            {
+                crabOn = b;
+                ApplyMode();
+                FleetDiag($"TOGGLE crab={b} -> mode={MultiVehicleManualMode}");
+            }
+        });
+
+        manip.AddWidget(new UseGesture.ToggleWidget
+        {
+            name = "fleet_rotate",
+            text = "原地旋转",
+            position = "50%+10px, 52%+10px",
+            size = "37.5%-10px, 10%-10px",
+            OnValue = b =>
+            {
+                rotateOn = b;
+                ApplyMode();
+                FleetDiag($"TOGGLE rotate={b} -> mode={MultiVehicleManualMode}");
+            }
+        });
+
+        manip.AddWidget(new UseGesture.ToggleWidget
+        {
+            name = "fleet_enable",
+            text = "车队联动",
+            position = "12.5%+10px, 64%+10px",
+            size = "37.5%-10px, 10%-10px",
+            OnValue = b =>
+            {
+                fleetOn = b;
+                MultiVehicleManualEnabled = b;
+                ApplyMode();
+                if (!b)
+                    ZeroFleetManualFields();
+                FleetDiag($"TOGGLE fleetOn={b} -> ManualEnabled={MultiVehicleManualEnabled} mode={MultiVehicleManualMode}");
+            }
+        });
+
+        manip.AddWidget(new UseGesture.ThrottleWidget
+        {
+            name = "fleet_speed_ratio",
+            text = "速度比例",
+            position = "50%+10px, 64%+10px",
+            size = "37.5%-10px, 10%-10px",
+            bounceBack = false,
+            OnValue = (val, _) => speedRatio = Math.Clamp(val, 0, 1)
+        });
+
+        manip.AddWidget(new UseGesture.ButtonWidget
+        {
+            name = "fleet_stop",
+            text = "急停/归零",
+            position = "31.25%+10px, 76%+10px",
+            size = "37.5%-10px, 10%-10px",
+            // OnPressed 是电平触发：每帧都会带当前按下状态回调一次（未按下时为 false）。
+            // 必须判断 pressed，否则会每帧无条件清零，把摇杆刚写入的速度立刻抹平 → 整车不动。
+            OnPressed = pressed =>
+            {
+                if (!pressed) return;
+                MultiVehicleManualVx = 0;
+                MultiVehicleManualVy = 0;
+                MultiVehicleManualVth = 0;
+                FleetDiag("BUTTON stop/zero");
+            }
+        });
+
        manip.ChangeState(new SetAppearance { drawGuizmo = false });
        manip.Start();

@@ -168,13 +212,14 @@ public partial class CartDefinition
            pb.Panel.TopMost(true)
                .SetDefaultDocking(Panel.Docking.None)
                .ShowTitle("车队联动遥控")
-                .InitSize(320, 160)
+                .InitSize(340, 190)
                .InitPos(false, -32, 32, 1, 0, 1, 0);

            pb.SeparatorText("状态");
-            pb.Label($"车队联动: {MultiVehicleManualEnabled}");
+            var modeName = MultiVehicleManualMode == 2 ? "原地旋转" : MultiVehicleManualMode == 1 ? "蟹行" : "常规";
+            pb.Label($"车队联动: {MultiVehicleManualEnabled}  模式: {modeName}");
            pb.Label($"速度比例: {speedRatio:0.00}");
-            pb.Label($"Vx={MultiVehicleManualVx:0.000} m/s, Vth={MultiVehicleManualVth:0.0} deg/s");
+            pb.Label($"Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} m/s, Vth={MultiVehicleManualVth:0.0}");
            pb.Label($"优先级: {CartActivator.CartDefinition.currentPriority} ({CartActivator.CartDefinition.currentPriorityDesc})");
            pb.Label("请勿同时打开「手动控制」面板");
            pb.Panel.Repaint();