First_Dev_Dept/Tutorial
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add dedicated fleet crab control parameters

Browse Source
This commit is contained in:
ruifeng.zhou
2026-07-01 23:10:12 +08:00
parent ca723fbdeb
commit 1c0258ec75
4 changed files with 83 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheel/MultiWheelC/MovementTests.cs
+47 -8
View File
@@ -387,6 +387,21 @@ public class FleetCrabWalk : MovementDefinition
/// <summary>行驶速度(m/s)。</summary>
public float CrabSpeed = 0.2f;
/// <summary>速度命令加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
public float FleetCrabAccel = 0.2f;
/// <summary>末端开始减速距离(mm)。</summary>
public float FleetCrabSlowDistance = 2000f;
/// <summary>完成距离(mm)，低于该剩余距离结束动作。</summary>
public float FleetCrabFinishDistance = 20f;
/// <summary>末端最低速度(m/s)。</summary>
public float FleetCrabFinishSpeed = 0.02f;
/// <summary>末端减速曲线指数。</summary>
public float FleetCrabSlowingPow = 0.8f;
/// <summary>前后 GCP 舵角修正上限(deg)。</summary>
public float GcpThetaThreshold = 95f;
@@ -429,6 +444,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<bool> Get()
{
var self = PilotDefinition.Self;
@@ -470,7 +493,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,10 +559,14 @@ 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 stopReason = "done";
@@ -558,12 +587,17 @@ public class FleetCrabWalk : MovementDefinition
if (remain <= finishDistance)
break;
var speed = baseSpeed;
var targetSpeed = baseSpeed;
if (remain < slowDistance)
{
var ratio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), conf.SlowingPow);
speed = ratio * (baseSpeed - finishSpeed) + finishSpeed;
var ratio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), slowingPow);
targetSpeed = ratio * (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);
@@ -600,7 +634,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}) " +
$"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");
@@ -648,6 +682,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());