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>
This commit is contained in:
Cursor
@@ -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();
|
||||
|
||||
Reference in New Issue
Block a user