Update fleet manual remote scaling
This commit is contained in:
@@ -6,15 +6,14 @@ namespace MultiWheelC;
|
|||||||
|
|
||||||
public class PilotConfig : MultiWheelPilotConfig
|
public class PilotConfig : MultiWheelPilotConfig
|
||||||
{
|
{
|
||||||
// ===== 多车联动(已从 MDCSToolBox 内联回 Tutorial)=====
|
[FieldMember(desc = "[sync] steering angle acceleration(deg/s^2)")] public float SyncThAccPerSec = 30f;
|
||||||
[FieldMember(desc = "[sync] 转向角加速度(deg/s^2)")] public float SyncThAccPerSec = 30f;
|
[FieldMember(desc = "[sync] fleet member distance(mm)")] public float TestCarSyncDistance = 2400f;
|
||||||
[FieldMember(desc = "[sync] 编队车间距(mm)")] public float TestCarSyncDistance = 2400f;
|
[FieldMember(desc = "[sync] fleet layout bias angle(deg)")] public float TestCarSyncTh = 0f;
|
||||||
[FieldMember(desc = "[sync] 编队布局偏角(deg)")] public float TestCarSyncTh = 0f;
|
// Fleet manual remote IO values are normalized joystick ratios. Keep all speed/angle scaling here.
|
||||||
// 手动遥控 Vx 已是 m/s、Vth 已是转向角(deg),此处系数保持 1(直通),不要再次缩放。
|
[FieldMember(desc = "[sync] fleet manual max linear speed(m/s)")] public float FleetManualMaxSpeed = 0.3f;
|
||||||
[FieldMember(desc = "[sync] 手动Vx系数")] public float ManualCarSyncVxFac = 1f;
|
[FieldMember(desc = "[sync] fleet manual normal-mode full-stick steering angle(deg)")] public float FleetManualMaxSteerAngleDeg = 45f;
|
||||||
// Manual crab mode: VxFac scales linear speed, VyFac maps steer stick ratio to crab steer angle in degrees.
|
[FieldMember(desc = "[sync] fleet manual crab-mode full-stick steering angle(deg)")] public float FleetManualMaxCrabAngleDeg = 60f;
|
||||||
[FieldMember(desc = "[sync] 手动Vy系数(蟹行满杆舵角deg)")] public float ManualCarSyncVyFac = 60f;
|
[FieldMember(desc = "[sync] fleet manual rotate-mode full-stick angular speed(deg/s)")] public float FleetManualMaxRotateOmegaDegPerSec = 45f;
|
||||||
[FieldMember(desc = "[sync] 手动Vth系数")] public float ManualCarSyncVthFac = 1f;
|
|
||||||
[FieldMember(desc = "[sync] 蟹行舵角上限(deg,应与Medulla舵轮角度限制匹配,默认120)")] public float MultiVehicleCrabSteerLimitDeg = 120f;
|
[FieldMember(desc = "[sync] 蟹行舵角上限(deg,应与Medulla舵轮角度限制匹配,默认120)")] public float MultiVehicleCrabSteerLimitDeg = 120f;
|
||||||
[FieldMember(desc = "[sync] 检测中心偏移(mm)")] public float DeltaDetectCenter = 350f;
|
[FieldMember(desc = "[sync] 检测中心偏移(mm)")] public float DeltaDetectCenter = 350f;
|
||||||
// 仅控制"车队内姿态纠正"(POS 补偿)是否使用 Detour 的 SLAM 位姿,不影响"整个车队姿态的计算"。
|
// 仅控制"车队内姿态纠正"(POS 补偿)是否使用 Detour 的 SLAM 位姿,不影响"整个车队姿态的计算"。
|
||||||
|
|||||||
@@ -37,14 +37,17 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
|||||||
[AsLowerIO(desc = "(手动)多车联动模式")] public int MultiVehicleManualMode;
|
[AsLowerIO(desc = "(手动)多车联动模式")] public int MultiVehicleManualMode;
|
||||||
[FieldMember(desc = "多车联动已同步")] public bool MultiVehicleAligned;
|
[FieldMember(desc = "多车联动已同步")] public bool MultiVehicleAligned;
|
||||||
|
|
||||||
[AsLowerIO(desc = "多车联动:遥控器Vx")] public float MultiVehicleManualVx;
|
[AsLowerIO(desc = "多车联动:遥控器Vx比例")] public float MultiVehicleManualVx;
|
||||||
[AsLowerIO(desc = "多车联动:遥控器Vy")] public float MultiVehicleManualVy;
|
[AsLowerIO(desc = "多车联动:遥控器Vy比例")] public float MultiVehicleManualVy;
|
||||||
[AsLowerIO(desc = "多车联动:遥控器Vth")] public float MultiVehicleManualVth;
|
[AsLowerIO(desc = "多车联动:遥控器Vth比例")] public float MultiVehicleManualVth;
|
||||||
|
[AsLowerIO(desc = "多车联动:遥控暂停")] public bool MultiVehicleHold;
|
||||||
|
|
||||||
// ===== Clumsy 侧脚本/动作驱动的手动等价输入(不走 Medulla IO,不会被 IO 同步覆盖)=====
|
// ===== Clumsy 侧脚本/动作驱动的手动等价输入(不走 Medulla IO,不会被 IO 同步覆盖)=====
|
||||||
// 手动 IO 字段是 [AsLowerIO](Medulla→Clumsy,Medulla 每周期回写),Clumsy 侧 MovementTest 写它们会被覆盖。
|
// 手动 IO 字段是 [AsLowerIO](Medulla→Clumsy,Medulla 每周期回写),Clumsy 侧 MovementTest 写它们会被覆盖。
|
||||||
// 因此提供这组内部字段,让 Clumsy 侧动作(如 FleetRotateInPlace)能像 FleetRemote 一样驱动车队联动:
|
// 因此提供这组内部字段,让 Clumsy 侧动作(如 FleetRotateInPlace)能像 FleetRemote 一样驱动车队联动。
|
||||||
// ScriptEnabled=使能;Mode 0=常规 1=蟹行 2=原地旋转;Vx/Vy/Vth 语义与手动遥控完全一致(m/s、m/s、deg/s)。
|
// ScriptEnabled=使能;Mode 0=常规 1=蟹行 2=原地旋转。
|
||||||
|
// 注意:Medulla 遥控 IO 是归一化摇杆比例;脚本字段保持物理量,避免动作参数再被 FleetManual* 二次缩放。
|
||||||
|
// Mode0: Vx=m/s, Vth=目标舵角deg;Mode1: Vx=m/s, Vy=蟹行舵角deg;Mode2: Vth=角速度deg/s。
|
||||||
[FieldMember(desc = "多车联动:脚本驱动使能")] public bool MultiVehicleScriptEnabled;
|
[FieldMember(desc = "多车联动:脚本驱动使能")] public bool MultiVehicleScriptEnabled;
|
||||||
[FieldMember(desc = "多车联动:脚本驱动模式")] public int MultiVehicleScriptMode;
|
[FieldMember(desc = "多车联动:脚本驱动模式")] public int MultiVehicleScriptMode;
|
||||||
[FieldMember(desc = "多车联动:脚本驱动Vx")] public float MultiVehicleScriptVx;
|
[FieldMember(desc = "多车联动:脚本驱动Vx")] public float MultiVehicleScriptVx;
|
||||||
@@ -214,7 +217,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
|||||||
|
|
||||||
DLog.Log(
|
DLog.Log(
|
||||||
$"INPUT master={isMaster} endpoint={Conf.MultiVehicleMasterEndpoint} selfEndpoint={Conf.MultiVehicleSelfEndpoint} car={CarNum} " +
|
$"INPUT master={isMaster} endpoint={Conf.MultiVehicleMasterEndpoint} selfEndpoint={Conf.MultiVehicleSelfEndpoint} car={CarNum} " +
|
||||||
$"rawEn={MultiVehicleManualEnabled} rawMode={MultiVehicleManualMode} rawVx={MultiVehicleManualVx:0.000} rawVy={MultiVehicleManualVy:0.000} rawVth={MultiVehicleManualVth:0.000} " +
|
$"rawEn={MultiVehicleManualEnabled} rawHold={MultiVehicleHold} rawMode={MultiVehicleManualMode} rawVx={MultiVehicleManualVx:0.000} rawVy={MultiVehicleManualVy:0.000} rawVth={MultiVehicleManualVth:0.000} " +
|
||||||
$"scriptOn={scriptOn} effEn={manualEnabled} effMode={manualMode} effVx={manualVx:0.000} effVy={manualVy:0.000} effVth={manualVth:0.000} " +
|
$"scriptOn={scriptOn} effEn={manualEnabled} effMode={manualMode} effVx={manualVx:0.000} effVy={manualVy:0.000} effVth={manualVth:0.000} " +
|
||||||
$"auto={autoEnabled} notifFresh={notifFresh} notifAgeMs={notifAgeMs:0} fleetCnt={fleetCnt}/{Conf.MultiVehicleFleetNum}",
|
$"auto={autoEnabled} notifFresh={notifFresh} notifAgeMs={notifAgeMs:0} fleetCnt={fleetCnt}/{Conf.MultiVehicleFleetNum}",
|
||||||
"MultiVehicleRemoteDbg");
|
"MultiVehicleRemoteDbg");
|
||||||
@@ -624,6 +627,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
|||||||
var manualVx = scriptOn ? MultiVehicleScriptVx : MultiVehicleManualVx;
|
var manualVx = scriptOn ? MultiVehicleScriptVx : MultiVehicleManualVx;
|
||||||
var manualVy = scriptOn ? MultiVehicleScriptVy : MultiVehicleManualVy;
|
var manualVy = scriptOn ? MultiVehicleScriptVy : MultiVehicleManualVy;
|
||||||
var manualVth = scriptOn ? MultiVehicleScriptVth : MultiVehicleManualVth;
|
var manualVth = scriptOn ? MultiVehicleScriptVth : MultiVehicleManualVth;
|
||||||
|
var manualHold = !scriptOn && MultiVehicleHold;
|
||||||
VehicleSyncNotification activeNotification = null;
|
VehicleSyncNotification activeNotification = null;
|
||||||
|
|
||||||
if (isMaster)
|
if (isMaster)
|
||||||
@@ -660,7 +664,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
|||||||
Math.Max(300, Conf.MultiVehicleSyncInterval * 5);
|
Math.Max(300, Conf.MultiVehicleSyncInterval * 5);
|
||||||
FleetDiag(
|
FleetDiag(
|
||||||
$"ENTRY master={isMaster} | IO: ManualEn={MultiVehicleManualEnabled} Mode={MultiVehicleManualMode} " +
|
$"ENTRY master={isMaster} | IO: ManualEn={MultiVehicleManualEnabled} Mode={MultiVehicleManualMode} " +
|
||||||
$"Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.0} AutoEn(IO)={MultiVehicleAutoEnabled} " +
|
$"Hold={MultiVehicleHold} Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.0} AutoEn(IO)={MultiVehicleAutoEnabled} " +
|
||||||
$"| SCRIPT en={MultiVehicleScriptEnabled} mode={MultiVehicleScriptMode} Vx={MultiVehicleScriptVx:0.000} Vy={MultiVehicleScriptVy:0.000} Vth={MultiVehicleScriptVth:0.0} " +
|
$"| SCRIPT en={MultiVehicleScriptEnabled} mode={MultiVehicleScriptMode} Vx={MultiVehicleScriptVx:0.000} Vy={MultiVehicleScriptVy:0.000} Vth={MultiVehicleScriptVth:0.0} " +
|
||||||
$"| gate: manualEnabled={manualEnabled} autoEnabled={autoEnabled} notifFresh={notifFresh} " +
|
$"| gate: manualEnabled={manualEnabled} autoEnabled={autoEnabled} notifFresh={notifFresh} " +
|
||||||
$"notifManualEn={(MultiVehicleNotification != null ? MultiVehicleNotification.ManualEnabled.ToString() : "null")} " +
|
$"notifManualEn={(MultiVehicleNotification != null ? MultiVehicleNotification.ManualEnabled.ToString() : "null")} " +
|
||||||
@@ -681,7 +685,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
|||||||
ResetMultiVehicleRotateCenterDrift();
|
ResetMultiVehicleRotateCenterDrift();
|
||||||
LogMultiVehicleRemoteDecision(
|
LogMultiVehicleRemoteDecision(
|
||||||
$"RETURN_IDLE master={isMaster} rawEn={MultiVehicleManualEnabled} scriptOn={scriptOn} auto={autoEnabled} " +
|
$"RETURN_IDLE master={isMaster} rawEn={MultiVehicleManualEnabled} scriptOn={scriptOn} auto={autoEnabled} " +
|
||||||
$"rawMode={MultiVehicleManualMode} rawVx={MultiVehicleManualVx:0.000} rawVy={MultiVehicleManualVy:0.000} rawVth={MultiVehicleManualVth:0.000}");
|
$"rawHold={MultiVehicleHold} rawMode={MultiVehicleManualMode} rawVx={MultiVehicleManualVx:0.000} rawVy={MultiVehicleManualVy:0.000} rawVth={MultiVehicleManualVth:0.000}");
|
||||||
UI.GetPainter("MultiVehicleFleet-vis", false).Clear();
|
UI.GetPainter("MultiVehicleFleet-vis", false).Clear();
|
||||||
sendMotionPainter.Clear();
|
sendMotionPainter.Clear();
|
||||||
lock (FleetLock)
|
lock (FleetLock)
|
||||||
@@ -762,10 +766,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
|||||||
{
|
{
|
||||||
syncTh = 0;
|
syncTh = 0;
|
||||||
fleetMode = manualMode;
|
fleetMode = manualMode;
|
||||||
|
var remoteRatioInput = !scriptOn;
|
||||||
if (fleetMode == 2)
|
if (fleetMode == 2)
|
||||||
{
|
{
|
||||||
// 原地旋转:摇杆左右 → 绕车队中心角速度(deg/s)。底盘 SetOriginBias 已设为车队中心。
|
// 外部遥控: Vth 为摇杆比例;内部脚本: Vth 为实际角速度(deg/s)。
|
||||||
fleetOmega = manualVth;
|
fleetOmega = remoteRatioInput
|
||||||
|
? ClampFloat(manualVth, -1f, 1f) * Math.Max(0f, Conf.FleetManualMaxRotateOmegaDegPerSec)
|
||||||
|
: manualVth;
|
||||||
fleetVx = 0;
|
fleetVx = 0;
|
||||||
fleetFrontTh = 0;
|
fleetFrontTh = 0;
|
||||||
fleetRearTh = 0;
|
fleetRearTh = 0;
|
||||||
@@ -774,12 +781,16 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
|||||||
}
|
}
|
||||||
else if (fleetMode == 1)
|
else if (fleetMode == 1)
|
||||||
{
|
{
|
||||||
// 手动蟹行:Vx 只表示线速度,Vy 表示方向摇杆比例(-1..1),由 VyFac 映射为舵角。
|
// 外部遥控: Vx/Vy 为摇杆比例;内部脚本: Vx 为线速度(m/s), Vy 为蟹行舵角(deg)。
|
||||||
var speed = manualVx * Conf.ManualCarSyncVxFac;
|
var speed = remoteRatioInput
|
||||||
var crabRatio = Math.Max(-90f, Math.Min(90f, manualVy));
|
? ClampFloat(manualVx, -1f, 1f) * Math.Max(0f, Conf.FleetManualMaxSpeed)
|
||||||
var crabAngle = crabRatio * Conf.ManualCarSyncVyFac;
|
: manualVx;
|
||||||
|
var crabRatio = remoteRatioInput ? ClampFloat(manualVy, -1f, 1f) : 0f;
|
||||||
|
var crabAngle = remoteRatioInput
|
||||||
|
? crabRatio * Math.Abs(Conf.FleetManualMaxCrabAngleDeg)
|
||||||
|
: manualVy;
|
||||||
crabInputVx = speed;
|
crabInputVx = speed;
|
||||||
crabInputVy = crabRatio;
|
crabInputVy = remoteRatioInput ? crabRatio : crabAngle;
|
||||||
crabRawAngle = crabAngle;
|
crabRawAngle = crabAngle;
|
||||||
crabSteerLimit = Math.Min(179f, Math.Max(1f, Math.Abs(Conf.MultiVehicleCrabSteerLimitDeg)));
|
crabSteerLimit = Math.Min(179f, Math.Max(1f, Math.Abs(Conf.MultiVehicleCrabSteerLimitDeg)));
|
||||||
|
|
||||||
@@ -793,8 +804,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
|||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
fleetVx = manualVx * Conf.ManualCarSyncVxFac;
|
// 外部遥控: Vx/Vth 为摇杆比例;内部脚本: Vx 为线速度(m/s), Vth 为目标舵角(deg)。
|
||||||
var targetTh = manualVth * Conf.ManualCarSyncVthFac;
|
fleetVx = remoteRatioInput
|
||||||
|
? ClampFloat(manualVx, -1f, 1f) * Math.Max(0f, Conf.FleetManualMaxSpeed)
|
||||||
|
: manualVx;
|
||||||
|
var targetTh = remoteRatioInput
|
||||||
|
? ClampFloat(manualVth, -1f, 1f) * Math.Abs(Conf.FleetManualMaxSteerAngleDeg)
|
||||||
|
: manualVth;
|
||||||
var now = DateTime.Now;
|
var now = DateTime.Now;
|
||||||
var dt = (float)Math.Min(0.2, Math.Max(0, (now - _multiVehicleLastThTime).TotalSeconds));
|
var dt = (float)Math.Min(0.2, Math.Max(0, (now - _multiVehicleLastThTime).TotalSeconds));
|
||||||
_multiVehicleLastThTime = now;
|
_multiVehicleLastThTime = now;
|
||||||
@@ -804,6 +820,17 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
|||||||
fleetFrontTh = _multiVehicleAccumulateTh;
|
fleetFrontTh = _multiVehicleAccumulateTh;
|
||||||
fleetRearTh = -fleetFrontTh;
|
fleetRearTh = -fleetFrontTh;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (manualHold)
|
||||||
|
{
|
||||||
|
fleetMode = 0;
|
||||||
|
fleetVx = 0;
|
||||||
|
fleetFrontTh = 0;
|
||||||
|
fleetRearTh = 0;
|
||||||
|
fleetOmega = 0;
|
||||||
|
_multiVehicleAccumulateTh = 0f;
|
||||||
|
_multiVehicleLastThTime = DateTime.Now;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
@@ -830,11 +857,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else if (MultiVehicleNotification != null)
|
else if (activeNotification != null)
|
||||||
{
|
{
|
||||||
VehicleSyncNotification notification;
|
var notification = activeNotification;
|
||||||
lock (_multiVehicleNotificationLock)
|
|
||||||
notification = MultiVehicleNotification;
|
|
||||||
|
|
||||||
syncTh = notification.SyncTh;
|
syncTh = notification.SyncTh;
|
||||||
syncDistance = notification.SyncDistance;
|
syncDistance = notification.SyncDistance;
|
||||||
|
|||||||
@@ -89,22 +89,25 @@ public partial class CartDefinition : CartActivator.CartDefinition
|
|||||||
[AsLowerIO(desc = "(手动)多车联动模式")]
|
[AsLowerIO(desc = "(手动)多车联动模式")]
|
||||||
public int MultiVehicleManualMode;
|
public int MultiVehicleManualMode;
|
||||||
|
|
||||||
[AsLowerIO(desc = "多车联动:遥控器Vx")]
|
[AsLowerIO(desc = "多车联动:遥控器Vx比例")]
|
||||||
public float MultiVehicleManualVx;
|
public float MultiVehicleManualVx;
|
||||||
|
|
||||||
[AsLowerIO(desc = "多车联动:遥控器Vy")]
|
[AsLowerIO(desc = "多车联动:遥控器Vy比例")]
|
||||||
public float MultiVehicleManualVy;
|
public float MultiVehicleManualVy;
|
||||||
|
|
||||||
[AsLowerIO(desc = "多车联动:遥控器Vth")]
|
[AsLowerIO(desc = "多车联动:遥控器Vth比例")]
|
||||||
public float MultiVehicleManualVth;
|
public float MultiVehicleManualVth;
|
||||||
|
|
||||||
[AsInitParam(desc = "手动最大速度(m/s)")]
|
[AsLowerIO(desc = "多车联动:遥控暂停")]
|
||||||
|
public bool MultiVehicleHold;
|
||||||
|
|
||||||
|
[AsInitParam(desc = "普通手动最大速度(m/s),车队联动不使用")]
|
||||||
public float MaxManualSpeed = 0.3f;
|
public float MaxManualSpeed = 0.3f;
|
||||||
|
|
||||||
[AsInitParam(desc = "手动最大自旋角速度(deg/s)")]
|
[AsInitParam(desc = "普通手动最大自旋角速度(deg/s),车队联动不使用")]
|
||||||
public float MaxManualAngularSpeed = 45f;
|
public float MaxManualAngularSpeed = 45f;
|
||||||
|
|
||||||
[AsInitParam(desc = "手动最大转向角度(deg)")]
|
[AsInitParam(desc = "普通手动最大转向角度(deg),车队联动不使用")]
|
||||||
public float MaxManualTheta = 45f;
|
public float MaxManualTheta = 45f;
|
||||||
|
|
||||||
[AsInitParam(desc = "遥控转向输入幂数")]
|
[AsInitParam(desc = "遥控转向输入幂数")]
|
||||||
|
|||||||
@@ -26,6 +26,7 @@ public partial class CartDefinition
|
|||||||
MultiVehicleManualVx = 0;
|
MultiVehicleManualVx = 0;
|
||||||
MultiVehicleManualVy = 0;
|
MultiVehicleManualVy = 0;
|
||||||
MultiVehicleManualVth = 0;
|
MultiVehicleManualVth = 0;
|
||||||
|
MultiVehicleHold = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
[IOObjectUtility]
|
[IOObjectUtility]
|
||||||
@@ -43,7 +44,6 @@ public partial class CartDefinition
|
|||||||
var crabOn = false; // 蟹行(四轮同向平移)
|
var crabOn = false; // 蟹行(四轮同向平移)
|
||||||
var rotateOn = false; // 原地旋转(绕车队中心)
|
var rotateOn = false; // 原地旋转(绕车队中心)
|
||||||
// 速度比例 0~1,作用于摇杆输出的线速度/横移/角速度。
|
// 速度比例 0~1,作用于摇杆输出的线速度/横移/角速度。
|
||||||
var speedRatio = 1f;
|
|
||||||
UseGesture? manip = null;
|
UseGesture? manip = null;
|
||||||
|
|
||||||
// 0=常规(前进+转向) 1=蟹行 2=原地旋转。crab 优先于 rotate(同时打开时蟹行生效)。
|
// 0=常规(前进+转向) 1=蟹行 2=原地旋转。crab 优先于 rotate(同时打开时蟹行生效)。
|
||||||
@@ -87,7 +87,6 @@ public partial class CartDefinition
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
var ratio = Math.Clamp(speedRatio, 0, 1);
|
|
||||||
var px = Math.Clamp(pos.X, -1, 1);
|
var px = Math.Clamp(pos.X, -1, 1);
|
||||||
var py = Math.Clamp(pos.Y, -1, 1);
|
var py = Math.Clamp(pos.Y, -1, 1);
|
||||||
var mode = CurrentMode();
|
var mode = CurrentMode();
|
||||||
@@ -95,31 +94,27 @@ public partial class CartDefinition
|
|||||||
|
|
||||||
if (mode == 2)
|
if (mode == 2)
|
||||||
{
|
{
|
||||||
// 原地旋转:pos.X(左右) → 角速度(deg/s),绕车队中心。
|
|
||||||
MultiVehicleManualVx = 0;
|
MultiVehicleManualVx = 0;
|
||||||
MultiVehicleManualVy = 0;
|
MultiVehicleManualVy = 0;
|
||||||
MultiVehicleManualVth = px * MaxManualAngularSpeed * ratio;
|
MultiVehicleManualVth = px;
|
||||||
}
|
}
|
||||||
else if (mode == 1)
|
else if (mode == 1)
|
||||||
{
|
{
|
||||||
// 蟹行:pos.Y → 前后向线速度,pos.X → 横向线速度(m/s)。
|
MultiVehicleManualVx = py;
|
||||||
MultiVehicleManualVx = py * MaxManualSpeed * ratio;
|
MultiVehicleManualVy = px;
|
||||||
MultiVehicleManualVy = px * MaxManualSpeed * ratio;
|
|
||||||
MultiVehicleManualVth = 0;
|
MultiVehicleManualVth = 0;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
// 常规:pos.Y → 线速度(m/s),pos.X → 转向角(deg)。
|
MultiVehicleManualVx = py;
|
||||||
MultiVehicleManualVx = py * MaxManualSpeed * ratio;
|
|
||||||
MultiVehicleManualVy = 0;
|
MultiVehicleManualVy = 0;
|
||||||
MultiVehicleManualVth = px * MaxManualAngularSpeed * ratio;
|
MultiVehicleManualVth = px;
|
||||||
}
|
}
|
||||||
|
|
||||||
if ((DateTime.Now - _fleetDiagLastStick).TotalMilliseconds >= 200)
|
if ((DateTime.Now - _fleetDiagLastStick).TotalMilliseconds >= 200)
|
||||||
{
|
{
|
||||||
_fleetDiagLastStick = DateTime.Now;
|
_fleetDiagLastStick = DateTime.Now;
|
||||||
FleetDiag($"STICK mode={mode} pos=({pos.X:0.00},{pos.Y:0.00}) manip={manipulating} ratio={ratio:0.00} " +
|
FleetDiag($"STICK mode={mode} pos=({pos.X:0.00},{pos.Y:0.00}) manip={manipulating} raw " +
|
||||||
$"-> Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.0} en={MultiVehicleManualEnabled}");
|
$"-> Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.000} en={MultiVehicleManualEnabled}");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
});
|
});
|
||||||
@@ -169,15 +164,6 @@ public partial class CartDefinition
|
|||||||
}
|
}
|
||||||
});
|
});
|
||||||
|
|
||||||
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
|
manip.AddWidget(new UseGesture.ButtonWidget
|
||||||
{
|
{
|
||||||
@@ -218,8 +204,7 @@ public partial class CartDefinition
|
|||||||
pb.SeparatorText("状态");
|
pb.SeparatorText("状态");
|
||||||
var modeName = MultiVehicleManualMode == 2 ? "原地旋转" : MultiVehicleManualMode == 1 ? "蟹行" : "常规";
|
var modeName = MultiVehicleManualMode == 2 ? "原地旋转" : MultiVehicleManualMode == 1 ? "蟹行" : "常规";
|
||||||
pb.Label($"车队联动: {MultiVehicleManualEnabled} 模式: {modeName}");
|
pb.Label($"车队联动: {MultiVehicleManualEnabled} 模式: {modeName}");
|
||||||
pb.Label($"速度比例: {speedRatio:0.00}");
|
pb.Label($"VxRatio={MultiVehicleManualVx:0.000} VyRatio={MultiVehicleManualVy:0.000}, VthRatio={MultiVehicleManualVth:0.000}");
|
||||||
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($"优先级: {CartActivator.CartDefinition.currentPriority} ({CartActivator.CartDefinition.currentPriorityDesc})");
|
||||||
pb.Label("请勿同时打开「手动控制」面板");
|
pb.Label("请勿同时打开「手动控制」面板");
|
||||||
pb.Panel.Repaint();
|
pb.Panel.Repaint();
|
||||||
|
|||||||
@@ -1,22 +0,0 @@
|
|||||||
using System.Threading.Tasks;
|
|
||||||
using SimpleComposer.RCS;
|
|
||||||
using SimpleCore.PropType;
|
|
||||||
|
|
||||||
namespace MultiWheelS
|
|
||||||
{
|
|
||||||
[CarType(Name = "多车联动AGV")]
|
|
||||||
public class MultiVehicleCar : GhostCar
|
|
||||||
{
|
|
||||||
public bool MultiVehicleSync = true;
|
|
||||||
|
|
||||||
public static new async Task<MultiVehicleCar> Create()
|
|
||||||
{
|
|
||||||
return new MultiVehicleCar
|
|
||||||
{
|
|
||||||
lstatus = "连接中",
|
|
||||||
address = "127.0.0.1",
|
|
||||||
name = "联动AGV"
|
|
||||||
};
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,62 +0,0 @@
|
|||||||
<?xml version="1.0" encoding="utf-8"?>
|
|
||||||
<Project ToolsVersion="15.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
|
||||||
<Import Project="$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props" Condition="Exists('$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props')" />
|
|
||||||
<PropertyGroup>
|
|
||||||
<LangVersion>latest</LangVersion>
|
|
||||||
</PropertyGroup>
|
|
||||||
<PropertyGroup>
|
|
||||||
<Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
|
|
||||||
<Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
|
|
||||||
<ProjectGuid>{D1D1D1D1-E2E2-F3F3-A4A4-B5B5B5B5B5B3}</ProjectGuid>
|
|
||||||
<OutputType>Library</OutputType>
|
|
||||||
<RootNamespace>MultiWheelS</RootNamespace>
|
|
||||||
<AssemblyName>MultiWheelS</AssemblyName>
|
|
||||||
<TargetFrameworkVersion>v4.8</TargetFrameworkVersion>
|
|
||||||
<FileAlignment>512</FileAlignment>
|
|
||||||
<Deterministic>true</Deterministic>
|
|
||||||
</PropertyGroup>
|
|
||||||
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
|
|
||||||
<DebugSymbols>true</DebugSymbols>
|
|
||||||
<DebugType>full</DebugType>
|
|
||||||
<Optimize>false</Optimize>
|
|
||||||
<OutputPath>bin\Debug\</OutputPath>
|
|
||||||
<DefineConstants>DEBUG;TRACE</DefineConstants>
|
|
||||||
<ErrorReport>prompt</ErrorReport>
|
|
||||||
<WarningLevel>4</WarningLevel>
|
|
||||||
</PropertyGroup>
|
|
||||||
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
|
|
||||||
<DebugType>pdbonly</DebugType>
|
|
||||||
<Optimize>true</Optimize>
|
|
||||||
<OutputPath>bin\Release\</OutputPath>
|
|
||||||
<DefineConstants>TRACE</DefineConstants>
|
|
||||||
<ErrorReport>prompt</ErrorReport>
|
|
||||||
<WarningLevel>4</WarningLevel>
|
|
||||||
</PropertyGroup>
|
|
||||||
<ItemGroup>
|
|
||||||
<Reference Include="LessokajiWeaverUtilities">
|
|
||||||
<HintPath>D:\MDCS\Release\deps\LessokajiWeaverUtilities.dll</HintPath>
|
|
||||||
</Reference>
|
|
||||||
<Reference Include="RefSimpleCore">
|
|
||||||
<HintPath>D:\MDCS\Dependencies\Simple\RefSimpleCore.dll</HintPath>
|
|
||||||
</Reference>
|
|
||||||
<Reference Include="SimpleComposer">
|
|
||||||
<HintPath>D:\MDCS\Executables\Simple\SimpleComposer.exe</HintPath>
|
|
||||||
</Reference>
|
|
||||||
<Reference Include="System" />
|
|
||||||
<Reference Include="System.Core" />
|
|
||||||
</ItemGroup>
|
|
||||||
<ItemGroup>
|
|
||||||
<Compile Include="MultiVehicleCar.cs" />
|
|
||||||
</ItemGroup>
|
|
||||||
<Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
|
|
||||||
<PropertyGroup>
|
|
||||||
<PostBuildEvent>if not exist "$(SolutionDir)build\Simple\plugins" mkdir "$(SolutionDir)build\Simple\plugins"
|
|
||||||
if not exist "$(SolutionDir)build\Simple" mkdir "$(SolutionDir)build\Simple"
|
|
||||||
xcopy "$(TargetDir)$(TargetFileName)" "$(SolutionDir)build\Simple\plugins" /y
|
|
||||||
xcopy "$(TargetDir)$(TargetName).pdb" "$(SolutionDir)build\Simple\plugins" /y
|
|
||||||
copy /Y "D:\MDCS\Executables\Simple\SimpleComposer.exe" "$(SolutionDir)build\Simple\"
|
|
||||||
copy /Y "D:\MDCS\Release\MDCSToolBox.dll" "$(SolutionDir)build\Simple\"
|
|
||||||
copy /Y "D:\MDCS\Release\CommonUsage.dll" "$(SolutionDir)build\Simple\"
|
|
||||||
copy /Y "D:\MDCS\Dependencies\Simple\RefSimpleCore.dll" "$(SolutionDir)build\Simple\"</PostBuildEvent>
|
|
||||||
</PropertyGroup>
|
|
||||||
</Project>
|
|
||||||
@@ -1,4 +1,3 @@
|
|||||||
|
|
||||||
Microsoft Visual Studio Solution File, Format Version 12.00
|
Microsoft Visual Studio Solution File, Format Version 12.00
|
||||||
# Visual Studio Version 17
|
# Visual Studio Version 17
|
||||||
VisualStudioVersion = 17.5.33424.131
|
VisualStudioVersion = 17.5.33424.131
|
||||||
@@ -15,8 +14,6 @@ Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "MultiWheelM", "MultiWheel\M
|
|||||||
EndProject
|
EndProject
|
||||||
Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "MultiWheelC", "MultiWheel\MultiWheelC\MultiWheelC.csproj", "{C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0}"
|
Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "MultiWheelC", "MultiWheel\MultiWheelC\MultiWheelC.csproj", "{C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0}"
|
||||||
EndProject
|
EndProject
|
||||||
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "MultiWheelS", "MultiWheel\MultiWheelS\MultiWheelS.csproj", "{D1D1D1D1-E2E2-F3F3-A4A4-B5B5B5B5B5B3}"
|
|
||||||
EndProject
|
|
||||||
Global
|
Global
|
||||||
GlobalSection(SolutionConfigurationPlatforms) = preSolution
|
GlobalSection(SolutionConfigurationPlatforms) = preSolution
|
||||||
Debug|Any CPU = Debug|Any CPU
|
Debug|Any CPU = Debug|Any CPU
|
||||||
@@ -39,10 +36,6 @@ Global
|
|||||||
{C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0}.Debug|Any CPU.Build.0 = Debug|Any CPU
|
{C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0}.Debug|Any CPU.Build.0 = Debug|Any CPU
|
||||||
{C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0}.Release|Any CPU.ActiveCfg = Release|Any CPU
|
{C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0}.Release|Any CPU.ActiveCfg = Release|Any CPU
|
||||||
{C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0}.Release|Any CPU.Build.0 = Release|Any CPU
|
{C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0}.Release|Any CPU.Build.0 = Release|Any CPU
|
||||||
{D1D1D1D1-E2E2-F3F3-A4A4-B5B5B5B5B5B3}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
|
|
||||||
{D1D1D1D1-E2E2-F3F3-A4A4-B5B5B5B5B5B3}.Debug|Any CPU.Build.0 = Debug|Any CPU
|
|
||||||
{D1D1D1D1-E2E2-F3F3-A4A4-B5B5B5B5B5B3}.Release|Any CPU.ActiveCfg = Release|Any CPU
|
|
||||||
{D1D1D1D1-E2E2-F3F3-A4A4-B5B5B5B5B5B3}.Release|Any CPU.Build.0 = Release|Any CPU
|
|
||||||
EndGlobalSection
|
EndGlobalSection
|
||||||
GlobalSection(SolutionProperties) = preSolution
|
GlobalSection(SolutionProperties) = preSolution
|
||||||
HideSolutionNode = FALSE
|
HideSolutionNode = FALSE
|
||||||
@@ -52,7 +45,6 @@ Global
|
|||||||
{A1B2C3D4-E5F6-7890-ABCD-EF1234567890} = {A1A1A1A1-B2B2-C3C3-D4D4-E5E5E5E5E5E1}
|
{A1B2C3D4-E5F6-7890-ABCD-EF1234567890} = {A1A1A1A1-B2B2-C3C3-D4D4-E5E5E5E5E5E1}
|
||||||
{B7B53B6B-98B5-4B6F-9B09-598F7B8166C9} = {A1A1A1A1-B2B2-C3C3-D4D4-E5E5E5E5E5E2}
|
{B7B53B6B-98B5-4B6F-9B09-598F7B8166C9} = {A1A1A1A1-B2B2-C3C3-D4D4-E5E5E5E5E5E2}
|
||||||
{C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0} = {A1A1A1A1-B2B2-C3C3-D4D4-E5E5E5E5E5E2}
|
{C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0} = {A1A1A1A1-B2B2-C3C3-D4D4-E5E5E5E5E5E2}
|
||||||
{D1D1D1D1-E2E2-F3F3-A4A4-B5B5B5B5B5B3} = {A1A1A1A1-B2B2-C3C3-D4D4-E5E5E5E5E5E2}
|
|
||||||
EndGlobalSection
|
EndGlobalSection
|
||||||
GlobalSection(ExtensibilityGlobals) = postSolution
|
GlobalSection(ExtensibilityGlobals) = postSolution
|
||||||
SolutionGuid = {EF6B92D5-5691-42DE-8B38-C17AE7DD7418}
|
SolutionGuid = {EF6B92D5-5691-42DE-8B38-C17AE7DD7418}
|
||||||
|
|||||||
@@ -94,17 +94,18 @@ TwoLegGuessX = -(TestCarSyncDistance - DeltaDetectCenter)
|
|||||||
|
|
||||||
## 4. 手动遥控(Medulla 车队遥控)
|
## 4. 手动遥控(Medulla 车队遥控)
|
||||||
|
|
||||||
遥控在 Medulla 侧产生指令(`MultiVehicleManual*`,`[AsLowerIO]` 上报 Clumsy)。摇杆输出已是物理单位,Clumsy 端系数应保持 **1(直通)**。
|
遥控在 Medulla 侧产生指令(`MultiVehicleManual*`,`[AsLowerIO]` 上报 Clumsy)。车队联动遥控只上报归一化摇杆比例 `[-1,1]`;实际速度/舵角/角速度统一在 Clumsy 的 `FleetManual*` 参数中换算,避免 Medulla 和 Clumsy 两层缩放叠加。
|
||||||
|
|
||||||
| 端 | 字段 | 含义 | 推荐值 |
|
| 端 | 字段 | 含义 | 推荐值 |
|
||||||
|----|------|------|--------|
|
|----|------|------|--------|
|
||||||
| Medulla `[AsInitParam]` | `MaxManualSpeed` | 车队手动**最大线速度(m/s)** | `0.3` |
|
| Medulla `[AsLowerIO]` | `MultiVehicleManualVx/Vy/Vth` | 车队遥控摇杆比例(仅 `[-1,1]`,不带物理单位) | 摇杆值 |
|
||||||
| Medulla `[AsInitParam]` | `MaxManualAngularSpeed` | 车队手动**最大转向角(deg)** | `45` |
|
| Clumsy `clumsy.json` | `FleetManualMaxSpeed` | 满杆线速度(m/s) | `0.3` |
|
||||||
| Clumsy `clumsy.json` | `ManualCarSyncVxFac` | 手动 Vx 系数(**保持 1,勿再缩放**) | `1.0` |
|
| Clumsy `clumsy.json` | `FleetManualMaxSteerAngleDeg` | 常规模式满杆转向舵角(deg) | `45` |
|
||||||
| Clumsy `clumsy.json` | `ManualCarSyncVthFac` | 手动 Vth 系数(**保持 1**) | `1.0` |
|
| Clumsy `clumsy.json` | `FleetManualMaxCrabAngleDeg` | 蟹行模式满杆蟹行舵角(deg) | `60` |
|
||||||
|
| Clumsy `clumsy.json` | `FleetManualMaxRotateOmegaDegPerSec` | 原地旋转模式满杆角速度(deg/s) | `45` |
|
||||||
| Clumsy `clumsy.json` | `SyncThAccPerSec` | 转向角爬升速率(deg/s) | `30` |
|
| Clumsy `clumsy.json` | `SyncThAccPerSec` | 转向角爬升速率(deg/s) | `30` |
|
||||||
|
|
||||||
> 历史坑:`ManualCarSyncVxFac=0.2 × MaxManualSpeed=0.3 → 0.06 m/s`,肉眼几乎不动;`VthFac=5 → 225°` 超舵轮范围。已统一为系数=1。
|
> 历史坑:旧链路会出现 `ManualCarSyncVxFac × MaxManualSpeed` 叠乘,导致满杆只有 `0.06 m/s` 这类异常低速;现在车队联动不再使用 Medulla 的 `MaxManualSpeed/MaxManualAngularSpeed`,这些字段只属于普通手动遥控。
|
||||||
|
|
||||||
操作:在 Medulla 打开车体工具 **「FleetRemote / 车队联动遥控」**(workspace 摇杆,松手自动回零),开「车队联动」开关后拖摇杆即可。主车摇杆驱动全队;从车由主车广播自动跟随,**无需**各自开开关。**不要**同时打开普通「手动控制」面板(会抢占优先级)。
|
操作:在 Medulla 打开车体工具 **「FleetRemote / 车队联动遥控」**(workspace 摇杆,松手自动回零),开「车队联动」开关后拖摇杆即可。主车摇杆驱动全队;从车由主车广播自动跟随,**无需**各自开开关。**不要**同时打开普通「手动控制」面板(会抢占优先级)。
|
||||||
|
|
||||||
@@ -212,8 +213,10 @@ TwoLegGuessX = -(TestCarSyncDistance - DeltaDetectCenter)
|
|||||||
"TwoLegGuessX": -1600,
|
"TwoLegGuessX": -1600,
|
||||||
"MultiVehicleFleetNum": 2,
|
"MultiVehicleFleetNum": 2,
|
||||||
"MultiVehicleUseDetect": true,
|
"MultiVehicleUseDetect": true,
|
||||||
"ManualCarSyncVxFac": 1.0,
|
"FleetManualMaxSpeed": 0.3,
|
||||||
"ManualCarSyncVthFac": 1.0,
|
"FleetManualMaxSteerAngleDeg": 45,
|
||||||
|
"FleetManualMaxCrabAngleDeg": 60,
|
||||||
|
"FleetManualMaxRotateOmegaDegPerSec": 45,
|
||||||
"SyncThAccPerSec": 30,
|
"SyncThAccPerSec": 30,
|
||||||
"MultiVehicleMasterEndpoint": "/", // 从车: "127.0.0.1:8008"
|
"MultiVehicleMasterEndpoint": "/", // 从车: "127.0.0.1:8008"
|
||||||
"PlaygroundRobotName": "agv_multi_1", // 从车: "agv_multi_2"
|
"PlaygroundRobotName": "agv_multi_1", // 从车: "agv_multi_2"
|
||||||
|
|||||||
Reference in New Issue
Block a user