diff --git a/MultiWheel/MultiWheelC/PilotConfig.cs b/MultiWheel/MultiWheelC/PilotConfig.cs index e7f9fdb..b4916f5 100644 --- a/MultiWheel/MultiWheelC/PilotConfig.cs +++ b/MultiWheel/MultiWheelC/PilotConfig.cs @@ -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; diff --git a/MultiWheel/MultiWheelC/PilotDefinition.cs b/MultiWheel/MultiWheelC/PilotDefinition.cs index 6f30af7..f6cb79c 100644 --- a/MultiWheel/MultiWheelC/PilotDefinition.cs +++ b/MultiWheel/MultiWheelC/PilotDefinition.cs @@ -64,30 +64,25 @@ public class PilotDefinition : MultiWheelPilotDefinition MultiVehicleLoop(hc)) { Name = "MultiVehicle", IsBackground = true }.Start(); } @@ -246,6 +243,9 @@ public class PilotDefinition : MultiWheelPilotDefinition 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 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; + var dt = (float)Math.Min(0.2, Math.Max(0, (now - _multiVehicleLastThTime).TotalSeconds)); + _multiVehicleLastThTime = now; + var sign = Math.Sign(targetTh - _multiVehicleAccumulateTh); + _multiVehicleAccumulateTh += sign * Math.Min(Math.Abs(targetTh - _multiVehicleAccumulateTh), + Conf.SyncThAccPerSec * dt); + fleetFrontTh = _multiVehicleAccumulateTh; + fleetRearTh = -fleetFrontTh; + } } else { @@ -382,6 +417,8 @@ public class PilotDefinition : MultiWheelPilotDefinition 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）=== @@ -544,7 +642,8 @@ public class PilotDefinition : MultiWheelPilotDefinition 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 Math.Sign(value) * Math.Min(Math.Abs(value), threshold); + /// + /// 原地旋转纠偏单轴 PI 控制器。err 为车体系偏差(mm 或 deg)，输出为修正速度(mm/s 或 deg/s)， + /// 带死区、积分抗饱和(限制积分贡献在 ±max 内)与总输出限幅。死区内冻结积分(保留稳态修正以抵消恒定扰动)。 + /// + 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); + } + + /// + /// 主车专用：通过 Playground WebAPI 读取本车与邻车的真实 sim 位姿，落 RotatePoseDbg 日志。 + /// 量化"开环横向滑移"来源： + /// - w1/w2/dw：两车实际偏航角速率(deg/s)。dw 持续非零 ⇒ 主从转速不一致(相对旋转)。 + /// - rel_in1(x,y,dyaw)：邻车在本车体系下的真实相对位姿；dyaw=实际相对朝向−180°。 + /// dyaw≈0 但 y 持续漂 ⇒ 纯横向平移滑移(非角度滞后，印证 B 无用)。 + /// - midDrift：车队几何中心(世界系)相对起转瞬间的位移 ⇒ 整体平移漂移量。 + /// 节流 ~200ms；WebAPI 异常时静默跳过，不影响控制环。 + /// + 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 } diff --git a/MultiWheel/MultiWheelC/VehicleSyncModels.cs b/MultiWheel/MultiWheelC/VehicleSyncModels.cs index ab69645..55369db 100644 --- a/MultiWheel/MultiWheelC/VehicleSyncModels.cs +++ b/MultiWheel/MultiWheelC/VehicleSyncModels.cs @@ -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; } diff --git a/MultiWheel/MultiWheelM/FleetRemote.cs b/MultiWheel/MultiWheelM/FleetRemote.cs index 6c84e25..ff8bd00 100644 --- a/MultiWheel/MultiWheelM/FleetRemote.cs +++ b/MultiWheel/MultiWheelM/FleetRemote.cs @@ -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; - MultiVehicleManualVy = 0; - MultiVehicleManualVth = Math.Clamp(pos.X, -1, 1) * MaxManualAngularSpeed * 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 = 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();