diff --git a/MultiWheel/MultiWheelC/MovementTests.cs b/MultiWheel/MultiWheelC/MovementTests.cs
index f09ed91..34c0f26 100644
--- a/MultiWheel/MultiWheelC/MovementTests.cs
+++ b/MultiWheel/MultiWheelC/MovementTests.cs
@@ -206,7 +206,11 @@ public class FleetRotateInPlace : MovementDefinition
var start = DateTime.Now;
var lastTime = start;
var lastLog = DateTime.MinValue;
+ var lastCenterLog = DateTime.MinValue;
var cmdMag = 0f; // 当前实际下发角速度大小(deg/s)，缓启动从 0 斜坡爬升
+ var centerTracking = false;
+ float centerStartX = 0, centerStartY = 0, centerStartTh = 0;
+ float centerLastX = 0, centerLastY = 0, centerLastTh = 0, centerMaxDrift = 0;
// 无定位按时长估算时，补上缓启动斜坡少转的等效时间(≈ maxOmega/(2·accel))，使时长更接近目标角。
var estDuration = maxOmega > 1e-3 ? targetMag / maxOmega : 0;
if (accel > 1e-3) estDuration += maxOmega / (2 * accel);
@@ -239,20 +243,43 @@ public class FleetRotateInPlace : MovementDefinition
yield return true;
}
+ float centerStartCarX = 0, centerStartCarY = 0, centerStartCarTh = 0;
if (hasPos)
{
- prevTh = (float)DetourInterface.getCartLocation().th;
+ var startPos = DetourInterface.getCartLocation();
+ centerStartCarX = (float)startPos.x;
+ centerStartCarY = (float)startPos.y;
+ centerStartCarTh = (float)startPos.th;
+ prevTh = centerStartCarTh;
startTh = prevTh;
+ if (self.TryGetFleetCenterFromPose(centerStartCarX, centerStartCarY, centerStartCarTh,
+ out centerStartX, out centerStartY, out centerStartTh))
+ {
+ centerLastX = centerStartX;
+ centerLastY = centerStartY;
+ centerLastTh = centerStartTh;
+ centerMaxDrift = 0;
+ centerTracking = true;
+ }
}
accumulated = 0f;
start = DateTime.Now;
lastTime = start;
lastLog = DateTime.MinValue;
+ lastCenterLog = DateTime.MinValue;
cmdMag = 0f;
DLog.Log(
$"START target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} startTh={startTh:0.00} " +
$"fleetAligned={self.MultiVehicleRotateFleetReady}",
"FleetRotateDbg");
+ if (centerTracking)
+ {
+ DLog.Log(
+ $"START center=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
+ $"car=({centerStartCarX:0.0},{centerStartCarY:0.0},{centerStartCarTh:0.00}) " +
+ $"target={TargetDeltaDeg:0.0} omega={maxOmega:0.0}",
+ "FleetRotateCenterDbg");
+ }
var stopReason = "stop()";
while (true)
@@ -266,7 +293,8 @@ public class FleetRotateInPlace : MovementDefinition
float curTh = 0f, remaining = 0f, actualRate = 0f;
if (hasPos)
{
- curTh = (float)DetourInterface.getCartLocation().th;
+ var carPos = DetourInterface.getCartLocation();
+ curTh = (float)carPos.th;
var step = (float)CommonMath.ThDiff(curTh, prevTh); // 本帧实际转角(逆时针为正)
accumulated += step;
actualRate = dt > 1e-3 ? step / dt : 0f; // 实际角速率(deg/s)，用于对比指令
@@ -280,6 +308,28 @@ public class FleetRotateInPlace : MovementDefinition
desiredMag = remaining < slowDeg
? Math.Max(minOmega, maxOmega * (remaining / slowDeg))
: maxOmega;
+
+ if (centerTracking &&
+ self.TryGetFleetCenterFromPose((float)carPos.x, (float)carPos.y, (float)carPos.th,
+ out centerLastX, out centerLastY, out centerLastTh))
+ {
+ var centerDx = centerLastX - centerStartX;
+ var centerDy = centerLastY - centerStartY;
+ var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
+ centerMaxDrift = Math.Max(centerMaxDrift, centerDrift);
+ var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
+ if ((now - lastCenterLog).TotalMilliseconds >= 250)
+ {
+ lastCenterLog = now;
+ DLog.Log(
+ $"ACTION t={elapsed:0.00}s center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
+ $"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
+ $"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
+ $"cmdW={dir * cmdMag:0.000} actualW={actualRate:0.000} acc={accumulated:0.0} remain={remaining:0.0} " +
+ $"wheelReady={self.MultiVehicleRotateWheelsReady} fleetReady={self.MultiVehicleRotateFleetReady}",
+ "FleetRotateCenterDbg");
+ }
+ }
}
else
{
@@ -329,6 +379,19 @@ public class FleetRotateInPlace : MovementDefinition
$"DONE reason={stopReason} 累计转角={accumulated:0.0}° 目标={TargetDeltaDeg:0.0}° " +
$"用时={(DateTime.Now - start).TotalSeconds:0.00}s useDetourHeading={hasPos}",
"FleetRotateDbg");
+ if (centerTracking)
+ {
+ var centerDx = centerLastX - centerStartX;
+ var centerDy = centerLastY - centerStartY;
+ var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
+ var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
+ DLog.Log(
+ $"DONE reason={stopReason} center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
+ $"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
+ $"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
+ $"acc={accumulated:0.0} target={TargetDeltaDeg:0.0}",
+ "FleetRotateCenterDbg");
+ }
Hedingben.ToastText($"车队原地旋转完成({stopReason}) 累计{accumulated:0.0}°", "FleetRotate");
}
}
@@ -387,6 +450,21 @@ public class FleetCrabWalk : MovementDefinition
/// 行驶速度(m/s)。
public float CrabSpeed = 0.2f;
+ /// 速度命令加速度限制(m/s^2)，小于等于 0 表示不限制。
+ public float FleetCrabAccel = 0.2f;
+
+ /// 末端开始减速距离(mm)。
+ public float FleetCrabSlowDistance = 2000f;
+
+ /// 完成距离(mm)，低于该剩余距离结束动作。
+ public float FleetCrabFinishDistance = 20f;
+
+ /// 末端最低速度(m/s)。
+ public float FleetCrabFinishSpeed = 0.02f;
+
+ /// 末端减速曲线指数。
+ public float FleetCrabSlowingPow = 0.8f;
+
/// 前后 GCP 舵角修正上限(deg)。
public float GcpThetaThreshold = 95f;
@@ -429,6 +507,14 @@ public class FleetCrabWalk : MovementDefinition
return value;
}
+ private static float Slew(float current, float target, float maxDelta)
+ {
+ if (maxDelta <= 0) return target;
+ if (target > current + maxDelta) return current + maxDelta;
+ if (target < current - maxDelta) return current - maxDelta;
+ return target;
+ }
+
public override IEnumerable Get()
{
var self = PilotDefinition.Self;
@@ -470,7 +556,9 @@ public class FleetCrabWalk : MovementDefinition
DLog.Log(
$"START center=({x0:0},{y0:0},{theta:0.0}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
$"phi={phi:0.0} targetBody={targetBodyTh:0.0} " +
- $"len={CrabLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000}",
+ $"len={CrabLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000} accel={FleetCrabAccel:0.000} " +
+ $"slow={FleetCrabSlowDistance:0} finishDist={FleetCrabFinishDistance:0} " +
+ $"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}",
"FleetCrabDbg");
self.MultiVehicleScriptEnabled = false;
@@ -534,11 +622,17 @@ public class FleetCrabWalk : MovementDefinition
var iter = 0;
var lastLog = DateTime.MinValue;
- var finishDistance = Math.Max(20f, conf.FinishDistance);
- var slowDistance = Math.Max(finishDistance + 1f, conf.SlowDistance);
+ var finishDistance = Math.Max(0f, FleetCrabFinishDistance);
+ var slowDistance = Math.Max(finishDistance + 1f, FleetCrabSlowDistance);
var baseSpeed = Math.Abs(CrabSpeed);
- var finishSpeed = Math.Min(baseSpeed, Math.Abs(conf.FinishSpeed));
+ var finishSpeed = Math.Min(baseSpeed, Math.Abs(FleetCrabFinishSpeed));
+ var slowingPow = Math.Max(0.01f, FleetCrabSlowingPow);
+ var accel = Math.Abs(FleetCrabAccel);
+ var cmdSpeed = 0f;
+ var lastTick = DateTime.Now;
var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
+ var holdFrontTh = ClampAbs((float)CommonMath.ThDiff(phi, theta), gcpLimit);
+ var holdRearTh = holdFrontTh;
var stopReason = "done";
while (!_stopping)
@@ -558,12 +652,18 @@ public class FleetCrabWalk : MovementDefinition
if (remain <= finishDistance)
break;
- var speed = baseSpeed;
+ var targetSpeed = baseSpeed;
+ var slowRatio = 1f;
if (remain < slowDistance)
{
- var ratio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), conf.SlowingPow);
- speed = ratio * (baseSpeed - finishSpeed) + finishSpeed;
+ slowRatio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), slowingPow);
+ targetSpeed = slowRatio * (baseSpeed - finishSpeed) + finishSpeed;
}
+ var now = DateTime.Now;
+ var dt = Math.Max(0.001f, (float)(now - lastTick).TotalSeconds);
+ lastTick = now;
+ var speed = accel > 0 ? Slew(cmdSpeed, targetSpeed, accel * dt) : targetSpeed;
+ cmdSpeed = speed;
var baseCrabTh = (float)CommonMath.ThDiff(phi, cth);
var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth);
@@ -572,6 +672,8 @@ public class FleetCrabWalk : MovementDefinition
biasItem = ClampAbs(biasItem, conf.BiasThreshold);
var frontTh = ClampAbs(baseCrabTh + biasItem + dthItem, gcpLimit);
var rearTh = ClampAbs(baseCrabTh + biasItem - dthItem, gcpLimit);
+ holdFrontTh = frontTh;
+ holdRearTh = rearTh;
var idealAlong = Clamp(along, 0f, CrabLengthMm);
var ideal = new Vector2(x0, y0) + pathDir * idealAlong;
@@ -600,7 +702,7 @@ public class FleetCrabWalk : MovementDefinition
$"ITER#{iter} center=({cx:0},{cy:0},{cth:0.0}) snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
$"along={along:0} lateral={lateral:0} remain={remain:0} headingErr={headingErr:0.0} " +
$"baseTh={baseCrabTh:0.0} bias={biasItem:0.0} dth={dthItem:0.0} " +
- $"auto=(vx:{speed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " +
+ $"slowRatio={slowRatio:0.000} targetV={targetSpeed:0.000} auto=(vx:{speed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " +
$"ideal=({ideal.X:0},{ideal.Y:0},{targetBodyTh:0.0}) scriptEn={self.MultiVehicleScriptEnabled} " +
$"cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
"FleetCrabDbg");
@@ -612,9 +714,12 @@ public class FleetCrabWalk : MovementDefinition
stopReason = "stop";
self.MultiVehicleAutoVx = 0;
- self.MultiVehicleAutoFrontTh = 0;
- self.MultiVehicleAutoRearTh = 0;
+ self.MultiVehicleAutoFrontTh = holdFrontTh;
+ self.MultiVehicleAutoRearTh = holdRearTh;
self.MultiVehicleAutoCmdTime = DateTime.Now;
+ DLog.Log(
+ $"STOP_HOLD iter={iter} reason={stopReason} hold=(fTh:{holdFrontTh:0.0},rTh:{holdRearTh:0.0}) cmdSpeed={cmdSpeed:0.000}",
+ "FleetCrabDbg");
var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
while (!_stopping && DateTime.Now < settleEnd)
{
@@ -622,8 +727,8 @@ public class FleetCrabWalk : MovementDefinition
self.MultiVehicleScriptMode = 0;
self.MultiVehicleAutoEnabled = true;
self.MultiVehicleAutoVx = 0;
- self.MultiVehicleAutoFrontTh = 0;
- self.MultiVehicleAutoRearTh = 0;
+ self.MultiVehicleAutoFrontTh = holdFrontTh;
+ self.MultiVehicleAutoRearTh = holdRearTh;
self.MultiVehicleAutoCmdTime = DateTime.Now;
yield return true;
}
@@ -648,6 +753,11 @@ public class FleetCrabWalkTest : MovementTest
BodyToPathAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
CrabLengthMm = PilotDefinition.Conf.FleetCrabLengthMm,
CrabSpeed = PilotDefinition.Conf.FleetCrabSpeed,
+ FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
+ FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
+ FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
+ FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,
+ FleetCrabSlowingPow = PilotDefinition.Conf.FleetCrabSlowingPow,
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
};
_task = new DriveTask(_proc.Get());
diff --git a/MultiWheel/MultiWheelC/PilotConfig.cs b/MultiWheel/MultiWheelC/PilotConfig.cs
index 912e607..7eb4116 100644
--- a/MultiWheel/MultiWheelC/PilotConfig.cs
+++ b/MultiWheel/MultiWheelC/PilotConfig.cs
@@ -87,10 +87,9 @@ public class PilotConfig : MultiWheelPilotConfig
// 仅当车队实际被指令旋转(|fleetOmega|超过此阈值)时才运行纠偏 PI；否则清零并复位积分，
// 避免松开摇杆后积分残留持续驱动车辆"自行旋转停不下来"。
[FieldMember(desc = "原地旋转纠偏：生效的最小角速度阈值(deg/s)")] public float MultiVehicleRotateActiveOmega = 0.5f;
- // 可选硬安全网：每轮纠偏速度幅值 ≤ 该比例×本轮旋转切向速度，限制合速度相对纯切向的最大偏角。
- // 默认 <0 关闭——纠偏随转速缩放(代码 #1)已让"纠偏:切向"比例全程恒定，匀速段不应再被削弱。
- // 仅在极端启动偏差导致匀速段仍乱打方向时，可设为 ~1.0(偏角≤45°) 兜底。
- [FieldMember(desc = "原地旋转纠偏：纠偏/旋转切向比例硬上限(默认-1关闭)")] public float MultiVehicleRotateCompTangentFrac = -1f;
+ // 安全网：每轮纠偏速度幅值 <= 该比例 * 本轮旋转切向速度，限制合速度相对纯切向的最大偏角。
+ // 旧配置若仍为 <0，运行时按安全默认 0.10 处理；确需放宽时可在主车显式调大并同步给从车。
+ [FieldMember(desc = "原地旋转纠偏：纠偏/旋转切向比例硬上限，<0使用安全默认0.10")] public float MultiVehicleRotateCompTangentFrac = 0.10f;
[FieldMember(desc = "单车同步 xy 精度(mm)")] public float SingleCarSyncPrecisionXy = 10f;
[FieldMember(desc = "单车同步 th 精度(deg)")] public float SingleCarSyncPrecisionTh = 0.2f;
@@ -125,6 +124,9 @@ public class PilotConfig : MultiWheelPilotConfig
[FieldMember(desc = "原地旋转：起转前舵轮对齐精度(deg)")]
public float InPlaceRotateWheelAlignDeg = 2f;
+ [FieldMember(desc = "原地旋转：旋转过程中舵轮偏差重对齐阈值(deg)")]
+ public float InPlaceRotateActiveWheelAlignDeg = 10f;
+
// ===== 车队联动-原地旋转动作（FleetRotateInPlace / 对应 FleetRemote 原地旋转模式）=====
// 通过 Clumsy 内部脚本字段驱动 TickMultiVehicle 的 mode2 旋转（绕车队中心 + PI 纠偏），需主车运行。
[FieldMember(desc = "车队原地旋转：角速度大小(deg/s，方向由目标角符号决定)")]
@@ -165,6 +167,21 @@ public class PilotConfig : MultiWheelPilotConfig
[FieldMember(desc = "车队蟹行：行驶速度(m/s)")]
public float FleetCrabSpeed = 0.2f;
+ [FieldMember(desc = "车队蟹行：速度命令加速度限制(m/s^2，<=0表示不限制)")]
+ public float FleetCrabAccel = 0.2f;
+
+ [FieldMember(desc = "车队蟹行：末端开始减速距离(mm)")]
+ public float FleetCrabSlowDistance = 2000f;
+
+ [FieldMember(desc = "车队蟹行：完成距离(mm)，低于该剩余距离结束动作")]
+ public float FleetCrabFinishDistance = 20f;
+
+ [FieldMember(desc = "车队蟹行：末端最低速度(m/s)")]
+ public float FleetCrabFinishSpeed = 0.02f;
+
+ [FieldMember(desc = "车队蟹行：末端减速曲线指数")]
+ public float FleetCrabSlowingPow = 0.8f;
+
[FieldMember(desc = "车队蟹行：GCP舵角修正上限(deg)")]
public float FleetCrabGcpThetaThreshold = 95f;
diff --git a/MultiWheel/MultiWheelC/PilotDefinition.cs b/MultiWheel/MultiWheelC/PilotDefinition.cs
index 4128399..bc7a704 100644
--- a/MultiWheel/MultiWheelC/PilotDefinition.cs
+++ b/MultiWheel/MultiWheelC/PilotDefinition.cs
@@ -146,12 +146,31 @@ public class PilotDefinition : MultiWheelPilotDefinition Conf.MultiVehicleRotateActiveOmega)
+ if (Math.Abs(requestedOmega) > rotateParams.ActiveOmega)
_multiVehicleRotateDirectionHint = Math.Sign(requestedOmega);
}
private bool PrepareMultiVehicleRotateWheels(MultiWheelChassis chassis, float requestedOmega,
+ RotateControlParams rotateParams,
float localCompensateX = 0f, float localCompensateY = 0f, float localCompensateTh = 0f,
bool rampStop = true)
{
- var hint = Math.Abs(requestedOmega) > Conf.MultiVehicleRotateActiveOmega
+ var hint = Math.Abs(requestedOmega) > rotateParams.ActiveOmega
? Math.Sign(requestedOmega)
: Math.Sign(_multiVehicleRotateDirectionHint);
if (hint == 0) hint = 1;
@@ -291,13 +312,13 @@ public class PilotDefinition : MultiWheelPilotDefinition Conf.MultiVehicleRotateActiveOmega
+ var alignOmega = Math.Abs(requestedOmega) > rotateParams.ActiveOmega
? requestedOmega
: 0.01f * hint;
var motionOk = chassis.SendRotateMotion(alignOmega, TimeSpan.Zero,
localCompensateX: localCompensateX, localCompensateY: localCompensateY,
localCompensateTh: localCompensateTh);
- var wheelAligned = TryCheckRotateWheelAlignment(chassis, Conf.InPlaceRotateWheelAlignDeg,
+ var wheelAligned = TryCheckRotateWheelAlignment(chassis, rotateParams.StartWheelAlignDeg,
out var alignDetail);
var aligned = motionOk && wheelAligned;
if (!motionOk)
@@ -315,13 +336,13 @@ public class PilotDefinition : MultiWheelPilotDefinition Conf.MultiVehicleRotateActiveOmega;
+ var rotating = Math.Abs(fleetOmega) > rotateParams.ActiveOmega;
if (canMove && rotating)
{
// #3 抗饱和：上一拍舵轮未对齐(gate=0、车没真正转动)时冻结积分，避免卡死时积分越积越大。
- var allowInteg = chassis.LastRotateAligned;
+ var allowInteg = chassis.LastRotateAligned &&
+ MultiVehicleRotateWheelsReady &&
+ MultiVehicleRotateFleetReady &&
+ !rotateHoldForAlignment;
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, allowInteg);
+ rotateParams.CompXyFac, rotateParams.CompXyIFac,
+ rotateParams.CompXyMax, dt, allowInteg);
rotCompVy = RotatePiTerm(errY, ref _rotIntegY, Conf.SingleCarSyncPrecisionXy,
- Conf.MultiVehicleRotateCompXyFac, Conf.MultiVehicleRotateCompXyIFac,
- Conf.MultiVehicleRotateCompXyMax, dt, allowInteg);
+ rotateParams.CompXyFac, rotateParams.CompXyIFac,
+ rotateParams.CompXyMax, dt, allowInteg);
rotCompOmega = RotatePiTerm(errTh, ref _rotIntegTh, Conf.SingleCarSyncPrecisionTh,
- Conf.MultiVehicleRotateCompThFac, Conf.MultiVehicleRotateCompThIFac,
- Conf.MultiVehicleRotateCompThMax, dt, allowInteg);
+ rotateParams.CompThFac, rotateParams.CompThIFac,
+ rotateParams.CompThMax, dt, allowInteg);
// #1 纠偏随旋转指令缩放：comp ×= |fleetOmega| / 本次峰值。
// 加速+匀速段峰值≈当前 → 系数≈1（全力纠偏，不削弱）；减速段当前<峰值 → 系数随转速同步下降。
@@ -1140,6 +1179,8 @@ public class PilotDefinition : MultiWheelPilotDefinition comp x:{xDetectCompensate:F1} y:{yDetectCompensate:F1} th:{thDetectCompensate:F2} " +
+ $"fac({Conf.MultiVehicleDetectBiasXFac:F2},{Conf.MultiVehicleDetectBiasYFac:F2},{Conf.MultiVehicleDetectBiasThFac:F2}) " +
+ $"lim({Conf.MultiVehicleDetectBiasXThreshold:F1},{Conf.MultiVehicleDetectBiasYThreshold:F1},{Conf.MultiVehicleDetectBiasThThreshold:F1}) " +
$"| 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} " +
+ $"fac({Conf.MultiVehiclePosBiasXFac:F2},{Conf.MultiVehiclePosBiasYFac:F2},{Conf.MultiVehiclePosBiasThFac:F2}) " +
+ $"lim({Conf.MultiVehiclePosBiasXThreshold:F1},{Conf.MultiVehiclePosBiasYThreshold:F1},{Conf.MultiVehiclePosBiasThThreshold:F1}) " +
$"| LAYOUT({layoutX:F0},{layoutY:F0},{layoutTh:F0}) R:{syncDistance / 2f:F0} " +
$"| 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}) " +
@@ -1341,6 +1412,7 @@ public class PilotDefinition : MultiWheelPilotDefinition Math.Sign(value) * Math.Min(Math.Abs(value), threshold);
+ private RotateControlParams BuildRotateControlParams(VehicleSyncNotification notification)
+ {
+ var parameters = new RotateControlParams
+ {
+ ActiveOmega = Conf.MultiVehicleRotateActiveOmega,
+ CompXyFac = Conf.MultiVehicleRotateCompXyFac,
+ CompXyIFac = Conf.MultiVehicleRotateCompXyIFac,
+ CompXyMax = Conf.MultiVehicleRotateCompXyMax,
+ CompThFac = Conf.MultiVehicleRotateCompThFac,
+ CompThIFac = Conf.MultiVehicleRotateCompThIFac,
+ CompThMax = Conf.MultiVehicleRotateCompThMax,
+ CompTangentFrac = NormalizeRotateCompTangentFrac(Conf.MultiVehicleRotateCompTangentFrac),
+ StartWheelAlignDeg = Conf.InPlaceRotateWheelAlignDeg,
+ ActiveWheelAlignDeg = NormalizeRotateWheelAlignDeg(Conf.InPlaceRotateActiveWheelAlignDeg,
+ Conf.InPlaceRotateWheelAlignDeg)
+ };
+
+ if (notification == null || !notification.RotateParamsValid)
+ return parameters;
+
+ parameters.ActiveOmega = notification.RotateActiveOmega;
+ parameters.CompXyFac = notification.RotateCompXyFac;
+ parameters.CompXyIFac = notification.RotateCompXyIFac;
+ parameters.CompXyMax = notification.RotateCompXyMax;
+ parameters.CompThFac = notification.RotateCompThFac;
+ parameters.CompThIFac = notification.RotateCompThIFac;
+ parameters.CompThMax = notification.RotateCompThMax;
+ parameters.CompTangentFrac = NormalizeRotateCompTangentFrac(notification.RotateCompTangentFrac);
+ parameters.StartWheelAlignDeg = NormalizeRotateWheelAlignDeg(notification.RotateStartWheelAlignDeg,
+ Conf.InPlaceRotateWheelAlignDeg);
+ parameters.ActiveWheelAlignDeg = NormalizeRotateWheelAlignDeg(notification.RotateActiveWheelAlignDeg,
+ parameters.StartWheelAlignDeg);
+ return parameters;
+ }
+
+ private static void FillRotateNotificationParams(VehicleSyncNotification notification, RotateControlParams parameters)
+ {
+ notification.RotateParamsValid = true;
+ notification.RotateActiveOmega = parameters.ActiveOmega;
+ notification.RotateCompXyFac = parameters.CompXyFac;
+ notification.RotateCompXyIFac = parameters.CompXyIFac;
+ notification.RotateCompXyMax = parameters.CompXyMax;
+ notification.RotateCompThFac = parameters.CompThFac;
+ notification.RotateCompThIFac = parameters.CompThIFac;
+ notification.RotateCompThMax = parameters.CompThMax;
+ notification.RotateCompTangentFrac = parameters.CompTangentFrac;
+ notification.RotateStartWheelAlignDeg = parameters.StartWheelAlignDeg;
+ notification.RotateActiveWheelAlignDeg = parameters.ActiveWheelAlignDeg;
+ }
+
+ private static float NormalizeRotateWheelAlignDeg(float value, float fallback)
+ {
+ if (float.IsNaN(value) || float.IsInfinity(value) || value <= 0)
+ return fallback;
+ return value;
+ }
+
+ private static float NormalizeRotateCompTangentFrac(float frac)
+ {
+ if (float.IsNaN(frac) || float.IsInfinity(frac) || frac < 0)
+ return DefaultRotateCompTangentFrac;
+ return frac;
+ }
+
+ private void LimitRotateCompensation(ref float compVx, ref float compVy, ref float compOmega,
+ float absOmega, float controlRadius, float tangentFrac)
+ {
+ var frac = NormalizeRotateCompTangentFrac(tangentFrac);
+ if (frac < 0 || absOmega <= 1e-6f) return;
+
+ var rawVx = compVx;
+ var rawVy = compVy;
+ var rawOmega = compOmega;
+ var tangentMmps = absOmega / 180f * (float)Math.PI * Math.Max(1f, Math.Abs(controlRadius));
+ var xyLimit = tangentMmps * frac;
+ var xyMag = (float)Math.Sqrt(compVx * compVx + compVy * compVy);
+ if (xyMag > xyLimit && xyMag > 1e-6f)
+ {
+ var scale = xyLimit / xyMag;
+ compVx *= scale;
+ compVy *= scale;
+ }
+
+ var omegaLimit = absOmega * frac;
+ compOmega = ClampBias(compOmega, omegaLimit);
+
+ var changed = Math.Abs(rawVx - compVx) > 1e-3f ||
+ Math.Abs(rawVy - compVy) > 1e-3f ||
+ Math.Abs(rawOmega - compOmega) > 1e-3f;
+ var now = DateTime.Now;
+ if (changed && (now - _mvRotateCompLimitLastLog).TotalMilliseconds >= 200)
+ {
+ _mvRotateCompLimitLastLog = now;
+ DLog.Log(
+ $"car{CarNum} ROTATE_COMP_LIMIT frac={frac:0.00} omega={absOmega:0.000} radius={controlRadius:0} " +
+ $"tan={tangentMmps:0.0} xyLimit={xyLimit:0.0} omLimit={omegaLimit:0.000} " +
+ $"raw=({rawVx:0.0},{rawVy:0.0},{rawOmega:0.000}) " +
+ $"limited=({compVx:0.0},{compVy:0.0},{compOmega:0.000})",
+ "MultiVehicleRemoteDbg");
+ }
+ }
+
+ private void ResetMultiVehicleRotateCenterDrift()
+ {
+ _mvRotateCenterDriftActive = false;
+ _mvRotateCenterMaxDrift = 0;
+ _mvRotateCenterLastLog = DateTime.MinValue;
+ }
+
+ private void LogMultiVehicleRotateCenterDrift(bool isMaster, bool manualEnabled, bool autoEnabled,
+ bool fleetReady, bool canMove, bool rotating, float centerX, float centerY, float centerTh,
+ float requestedOmega, float fleetOmega, float compVx, float compVy, float compOmega)
+ {
+ if (!_mvRotateCenterDriftActive)
+ {
+ _mvRotateCenterDriftActive = true;
+ _mvRotateCenterStartX = centerX;
+ _mvRotateCenterStartY = centerY;
+ _mvRotateCenterStartTh = centerTh;
+ _mvRotateCenterMaxDrift = 0;
+ _mvRotateCenterLastLog = DateTime.MinValue;
+ }
+
+ var dx = centerX - _mvRotateCenterStartX;
+ var dy = centerY - _mvRotateCenterStartY;
+ var drift = (float)Math.Sqrt(dx * dx + dy * dy);
+ _mvRotateCenterMaxDrift = Math.Max(_mvRotateCenterMaxDrift, drift);
+ var dth = (float)CommonMath.ThDiff(centerTh, _mvRotateCenterStartTh);
+ var now = DateTime.Now;
+ if ((now - _mvRotateCenterLastLog).TotalMilliseconds < 250)
+ return;
+
+ _mvRotateCenterLastLog = now;
+ DLog.Log(
+ $"car{CarNum} CTRL master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
+ $"ready={fleetReady} canMove={canMove} rotating={rotating} " +
+ $"center=({centerX:0.0},{centerY:0.0},{centerTh:0.00}) " +
+ $"start=({_mvRotateCenterStartX:0.0},{_mvRotateCenterStartY:0.0},{_mvRotateCenterStartTh:0.00}) " +
+ $"drift=({dx:0.0},{dy:0.0}) dist={drift:0.0} max={_mvRotateCenterMaxDrift:0.0} dth={dth:0.00} " +
+ $"omegaReq={requestedOmega:0.000} omega={fleetOmega:0.000} comp=({compVx:0.0},{compVy:0.0},{compOmega:0.000})",
+ "FleetRotateCenterDbg");
+ }
+
///
/// 原地旋转纠偏单轴 PI 控制器。err 为车体系偏差(mm 或 deg)，输出为修正速度(mm/s 或 deg/s)，
/// 带死区、积分抗饱和(限制积分贡献在 ±max 内)与总输出限幅。死区内冻结积分(保留稳态修正以抵消恒定扰动)。
diff --git a/MultiWheel/MultiWheelC/VehicleSyncBinaryCodec.cs b/MultiWheel/MultiWheelC/VehicleSyncBinaryCodec.cs
index 11ef343..e557edc 100644
--- a/MultiWheel/MultiWheelC/VehicleSyncBinaryCodec.cs
+++ b/MultiWheel/MultiWheelC/VehicleSyncBinaryCodec.cs
@@ -55,6 +55,16 @@ internal static class VehicleSyncBinaryCodec
writer.Write(notification.SyncTh);
writer.Write(notification.SyncDistance);
writer.Write(notification.DeltaDetectCenter);
+ writer.Write(notification.RotateActiveOmega);
+ writer.Write(notification.RotateCompXyFac);
+ writer.Write(notification.RotateCompXyIFac);
+ writer.Write(notification.RotateCompXyMax);
+ writer.Write(notification.RotateCompThFac);
+ writer.Write(notification.RotateCompThIFac);
+ writer.Write(notification.RotateCompThMax);
+ writer.Write(notification.RotateCompTangentFrac);
+ writer.Write(notification.RotateStartWheelAlignDeg);
+ writer.Write(notification.RotateActiveWheelAlignDeg);
writer.Write(notification.IdealX);
writer.Write(notification.IdealY);
writer.Write(notification.IdealTh);
@@ -99,6 +109,16 @@ internal static class VehicleSyncBinaryCodec
notification.SyncTh = reader.ReadSingle();
notification.SyncDistance = reader.ReadSingle();
notification.DeltaDetectCenter = reader.ReadSingle();
+ notification.RotateActiveOmega = reader.ReadSingle();
+ notification.RotateCompXyFac = reader.ReadSingle();
+ notification.RotateCompXyIFac = reader.ReadSingle();
+ notification.RotateCompXyMax = reader.ReadSingle();
+ notification.RotateCompThFac = reader.ReadSingle();
+ notification.RotateCompThIFac = reader.ReadSingle();
+ notification.RotateCompThMax = reader.ReadSingle();
+ notification.RotateCompTangentFrac = reader.ReadSingle();
+ notification.RotateStartWheelAlignDeg = reader.ReadSingle();
+ notification.RotateActiveWheelAlignDeg = reader.ReadSingle();
notification.IdealX = reader.ReadSingle();
notification.IdealY = reader.ReadSingle();
notification.IdealTh = reader.ReadSingle();
@@ -209,6 +229,8 @@ internal static class VehicleSyncBinaryCodec
if (notification.AutoEnabled) flags |= 1 << 4;
if (notification.ManualEnabled) flags |= 1 << 5;
if (notification.HasIdeal) flags |= 1 << 6;
+ if (notification.RotateParamsValid) flags |= 1 << 7;
+ if (notification.UseDetourCorrection) flags |= 1 << 8;
return flags;
}
@@ -221,6 +243,8 @@ internal static class VehicleSyncBinaryCodec
notification.AutoEnabled = (flags & (1 << 4)) != 0;
notification.ManualEnabled = (flags & (1 << 5)) != 0;
notification.HasIdeal = (flags & (1 << 6)) != 0;
+ notification.RotateParamsValid = (flags & (1 << 7)) != 0;
+ notification.UseDetourCorrection = (flags & (1 << 8)) != 0;
}
private static void WriteString(BinaryWriter writer, string value)
diff --git a/MultiWheel/MultiWheelC/VehicleSyncModels.cs b/MultiWheel/MultiWheelC/VehicleSyncModels.cs
index 8c8e7b1..9f5ca3f 100644
--- a/MultiWheel/MultiWheelC/VehicleSyncModels.cs
+++ b/MultiWheel/MultiWheelC/VehicleSyncModels.cs
@@ -47,9 +47,22 @@ public class VehicleSyncNotification
[JsonProperty("FleetStopSourceCar")] public int FleetStopSourceCar { get; set; }
[JsonProperty("AutoEnabled")] public bool AutoEnabled { get; set; }
[JsonProperty("ManualEnabled")] public bool ManualEnabled { get; set; }
+ [JsonProperty("UseDetourCorrection")] public bool UseDetourCorrection { get; set; }
[JsonProperty("SyncTh")] public float SyncTh { get; set; }
[JsonProperty("SyncDistance")] public float SyncDistance { get; set; }
[JsonProperty("DeltaDetectCenter")] public float DeltaDetectCenter { get; set; }
+ // 原地旋转纠偏参数由主车广播，从车运行时使用同一套增益/限幅，避免主从补偿强度不一致。
+ [JsonProperty("RotateParamsValid")] public bool RotateParamsValid { get; set; }
+ [JsonProperty("RotateActiveOmega")] public float RotateActiveOmega { get; set; }
+ [JsonProperty("RotateCompXyFac")] public float RotateCompXyFac { get; set; }
+ [JsonProperty("RotateCompXyIFac")] public float RotateCompXyIFac { get; set; }
+ [JsonProperty("RotateCompXyMax")] public float RotateCompXyMax { get; set; }
+ [JsonProperty("RotateCompThFac")] public float RotateCompThFac { get; set; }
+ [JsonProperty("RotateCompThIFac")] public float RotateCompThIFac { get; set; }
+ [JsonProperty("RotateCompThMax")] public float RotateCompThMax { get; set; }
+ [JsonProperty("RotateCompTangentFrac")] public float RotateCompTangentFrac { get; set; }
+ [JsonProperty("RotateStartWheelAlignDeg")] public float RotateStartWheelAlignDeg { get; set; }
+ [JsonProperty("RotateActiveWheelAlignDeg")] public float RotateActiveWheelAlignDeg { get; set; }
// F: 单调递增序列号，从车据此丢弃乱序到达的旧 notify 包。
[JsonProperty("Seq")] public long Seq { get; set; }
// D: 自动模式下主车路径控制器算出的车队中心理想位姿（世界系），由 idealPos/idealAngle 透传而来。
diff --git a/docs/FleetCrabWalkWorkContext.md b/docs/FleetCrabWalkWorkContext.md
index 0970702..57ef759 100644
--- a/docs/FleetCrabWalkWorkContext.md
+++ b/docs/FleetCrabWalkWorkContext.md
@@ -270,10 +270,15 @@ MovementTest「车队联动-自动蟹行」
|------|------|------|
| `FleetCrabAngleDeg` | 45 | 路径方向相对启动时车队朝向的夹角 (deg)。MovementTest 同时把车身-路径夹角设为该值；若输入“路径与小车夹角 x 度”，应填 `-x` 以保持当前车身角度 |
| `FleetCrabLengthMm` | 2000 | 路径长度 (mm) |
-| `FleetCrabSpeed` | 0.2 | 巡航速度 (m/s)，接近终点时由通用减速参数下调 |
+| `FleetCrabSpeed` | 0.2 | 巡航速度 (m/s)，接近终点时由自动蟹行专用减速参数下调 |
+| `FleetCrabAccel` | 0.2 | 速度命令加速度限制 (m/s^2)，限制 `MultiVehicleAutoVx` 每拍变化量；`<=0` 表示不限制 |
+| `FleetCrabSlowDistance` | 2000 | 末端开始减速距离 (mm) |
+| `FleetCrabFinishDistance` | 20 | 完成距离 (mm)，剩余距离低于该值时结束动作 |
+| `FleetCrabFinishSpeed` | 0.02 | 末端最低速度 (m/s) |
+| `FleetCrabSlowingPow` | 0.8 | 末端减速曲线指数；越大越靠近终点才明显降速，越小越早降速 |
| `FleetCrabGcpThetaThreshold` | 95 | 自动蟹行输出 `frontTh/rearTh` 的绝对值上限，应给实际舵角限位与 `AngleLimitMarginDeg` 留余量 |
-已删除旧字段：`FleetCrabCorrectionGain`、`FleetCrabCorrectionAngleDeg`、`FleetCrabCommandAccel`。旧 `clumsy.json` 若残留这些 key，会被配置反序列化忽略，不能再作为有效调参项。
+已删除旧字段：`FleetCrabCorrectionGain`、`FleetCrabCorrectionAngleDeg`、`FleetCrabCommandAccel`。旧 `clumsy.json` 若残留这些 key，会被配置反序列化忽略；新的自动链路使用 `FleetCrabAccel` 控制 `MultiVehicleAutoVx` 速度命令斜率。
动作行为：当前不再改 `MultiVehicleAutoUseIdealCenter`，结束/急停会清零 `MultiVehicleAuto*`，并保持 `MultiVehicleScriptEnabled=false`。
@@ -288,7 +293,7 @@ MovementTest「车队联动-自动蟹行」
| `DriveTaskInterval` | 50 | `clumsy.json` 顶层 |
| `BiasFac` / `BiasThreshold` | 继承 `MultiWheelPilotConfig` | 横向偏差 `lateral` → 前后 GCP 同向修正 |
| `DthLinearFac` / `DthLinearThreshold` | 继承 `MultiWheelPilotConfig` | 车身目标朝向偏差 `headingErr` → 前后 GCP 反向修正 |
-| `SlowDistance` / `SlowingPow` / `FinishDistance` / `FinishSpeed` | 继承 `BasicPilotConfig` | 自动蟹行终点减速和结束判定 |
+| `FleetCrabSlowDistance` / `FleetCrabSlowingPow` / `FleetCrabFinishDistance` / `FleetCrabFinishSpeed` | `PilotConfig` | 自动蟹行专用终点减速和结束判定 |
| `MultiVehicleAutoUseIdealCenter` | true（默认） | 使用自动蟹行发布的 ideal center 给从车做前馈 |
| `MultiVehicleAutoRequireFleetCenter` | true（默认） | 自动模式无有效车队中心时整队停车 |
| `MultiVehicleAutoCmdTimeoutMs` | 0（auto） | 自动命令新鲜度超时，避免控制器停发后沿末速度滑行 |
diff --git a/docs/MultiVehicleConfig.md b/docs/MultiVehicleConfig.md
index 75c8f0e..25403c2 100644
--- a/docs/MultiVehicleConfig.md
+++ b/docs/MultiVehicleConfig.md
@@ -155,7 +155,12 @@ TwoLegGuessX = -(TestCarSyncDistance - DeltaDetectCenter)
|------|------|--------|
| `FleetCrabAngleDeg` | 蟹行路径方向相对**启动时车队朝向**的夹角(deg，逆时针为正)。MovementTest 同时把车身-路径夹角设为该值，因此 `FleetCrabAngleDeg=-x` 会让车身保持启动朝向，并以 `x` 度夹角追踪路径 | `45` |
| `FleetCrabLengthMm` | 蟹行路径**长度**(mm)，沿夹角方向行驶该距离后停车结束 | `2000` |
-| `FleetCrabSpeed` | 蟹行巡航速度(m/s)，写入 `MultiVehicleAutoVx`；接近终点时会被 `SlowDistance/SlowingPow/FinishSpeed` 降速 | `0.2` |
+| `FleetCrabSpeed` | 蟹行巡航速度(m/s)，写入 `MultiVehicleAutoVx`；接近终点时会被自动蟹行专用减速参数下调 | `0.2` |
+| `FleetCrabAccel` | 蟹行速度命令加速度限制(m/s^2)，限制 `MultiVehicleAutoVx` 每拍变化量；`<=0` 表示不限制 | `0.2` |
+| `FleetCrabSlowDistance` | 蟹行末端开始减速距离(mm) | `2000` |
+| `FleetCrabFinishDistance` | 蟹行完成距离(mm)，剩余距离低于该值时结束动作 | `20` |
+| `FleetCrabFinishSpeed` | 蟹行末端最低速度(m/s) | `0.02` |
+| `FleetCrabSlowingPow` | 蟹行末端减速曲线指数；越大越靠近终点才明显降速，越小越早降速 | `0.8` |
| `FleetCrabGcpThetaThreshold` | 自动蟹行输出 `frontTh/rearTh` 的绝对值上限(deg)。应小于实际舵角可行范围，并给 `AngleLimitMarginDeg` 留余量 | `95` |
自动蟹行还会使用下列通用控制参数：
@@ -164,7 +169,6 @@ TwoLegGuessX = -(TestCarSyncDistance - DeltaDetectCenter)
|------|------|----------|
| `BiasFac` / `BiasThreshold` | 横向偏差 `lateral` → 前后 GCP 同向修正。增大后收敛更快，但过大可能摆动 | `MultiWheelPilotConfig` |
| `DthLinearFac` / `DthLinearThreshold` | 车身目标朝向偏差 `headingErr` → 前后 GCP 反向修正，用于保持车身与路径夹角 | `MultiWheelPilotConfig` |
-| `SlowDistance` / `SlowingPow` / `FinishDistance` / `FinishSpeed` | 终点减速和结束判定 | `BasicPilotConfig` |
| `MultiVehicleAutoUseIdealCenter` | 是否把自动蟹行计算出的 `IdealX/Y/Th` 广播给从车做前馈 | `true` |
| `MultiVehicleAutoRequireFleetCenter` | 自动模式是否要求有效车队中心；定位/车队中心失效时整队停车 | `true` |
| `MultiVehicleAutoCmdTimeoutMs` | 自动命令新鲜度超时，0 表示按联动周期自动计算 | `0` |
@@ -177,12 +181,12 @@ TwoLegGuessX = -(TestCarSyncDistance - DeltaDetectCenter)
|------------|--------|----------|
| `FleetCrabCorrectionGain` | 旧脚本链路的横向误差纠偏增益 | `BiasFac` |
| `FleetCrabCorrectionAngleDeg` | 旧脚本链路的最大改向角 | `BiasThreshold` / `FleetCrabGcpThetaThreshold` |
-| `FleetCrabCommandAccel` | 旧脚本链路的 `Vx/Vy` 斜率限制 | 由自动联动周期、底盘速度斜坡和终点减速共同约束 |
+| `FleetCrabCommandAccel` | 旧脚本链路的 `Vx/Vy` 斜率限制 | 新自动链路使用 `FleetCrabAccel` 限制 `MultiVehicleAutoVx` |
**行为要点 / 注意**
- 当前实现走 `MultiVehicleAuto*` 自动字段链路，不再开启 `MultiVehicleScriptEnabled`，也不受 `MultiVehicleCrabSteerLimitDeg` 影响（该字段只影响手动 `mode=1` 蟹行）。
-- `FleetCrabDbg` 会记录 `along/lateral/remain/headingErr/baseTh/bias/dth/auto(vx,fTh,rTh)/ideal`；`MultiVehicleDbg` 可继续对照最终 `BASE/SEND`、POS/Detect 补偿、ready/stop 状态。
+- `FleetCrabDbg` 会记录 `along/lateral/remain/headingErr/baseTh/bias/dth/targetV/auto(vx,fTh,rTh)/ideal`；`MultiVehicleDbg` 可继续对照最终 `BASE/SEND`、POS/Detect 补偿、ready/stop 状态。
- `MultiVehicleUseDetect=true` 时仍受 2 腿检测安全门约束（检测丢失会被置零停车）。
- Playground 双车场景的 `actuator.maxSteeringAngle` 也必须与该上限一致；若仍为 `90`，Clumsy 发出的 `-98°` 纠偏会在仿真执行层被夹回 `-90°`，表现为纯横移路径无法收敛。
@@ -219,6 +223,11 @@ TwoLegGuessX = -(TestCarSyncDistance - DeltaDetectCenter)
"FleetCrabAngleDeg": 45,
"FleetCrabLengthMm": 2000,
"FleetCrabSpeed": 0.2,
+"FleetCrabAccel": 0.2,
+"FleetCrabSlowDistance": 2000,
+"FleetCrabFinishDistance": 20,
+"FleetCrabFinishSpeed": 0.02,
+"FleetCrabSlowingPow": 0.8,
"FleetCrabGcpThetaThreshold": 95
```