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

Add FleetCrabWalk startup motion config

Browse Source
This commit is contained in:
ruifeng.zhou
2026-07-04 18:11:21 +08:00
parent e5ef701729
commit 980ee9170b
3 changed files with 34 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheel/MultiWheelC/MovementTests.cs
+16 -8
View File
@@ -453,6 +453,9 @@ public class FleetCrabWalk : MovementDefinition
/// <summary>速度命令加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
public float FleetCrabAccel = 0.2f;
/// <summary>预对齐后正式下发速度前 5 秒加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
public float FleetCrabStartAccel = 0.01f;
/// <summary>末端开始减速距离(mm)。</summary>
public float FleetCrabSlowDistance = 2000f;
@@ -657,7 +660,7 @@ public class FleetCrabWalk : MovementDefinition
$"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " +
$"autoFields=true pathMode=relative pathAngle={CrabAngleDeg:0.0} " +
$"bodyToPath={BodyToPathAngleDeg:0.0} gcpLimit={GcpThetaThreshold:0.0} " +
$"biasFac={conf.BiasFac:0.00} dthFac={conf.DthLinearFac:0.00}",
$"biasFac={conf.BiasFac:0.00} fleetCrabDthFac={conf.FleetCrabDthLinearFac:0.00}",
"FleetCrabDbg");
if (conf.MultiVehicleMasterEndpoint != "/")
@@ -692,7 +695,7 @@ public class FleetCrabWalk : MovementDefinition
$"START center=({x0:0},{y0:0},{theta:0.0}) pathMode=relative " +
$"src=({pathStart.X:0},{pathStart.Y:0}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
$"phi={phi:0.0} targetBody={targetBodyTh:0.0} " +
$"len={pathLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000} accel={FleetCrabAccel:0.000} " +
$"len={pathLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000} startAccel={FleetCrabStartAccel:0.000} accel={FleetCrabAccel:0.000} " +
$"slow={FleetCrabSlowDistance:0} finishDist={FleetCrabFinishDistance:0} " +
$"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}",
"FleetCrabDbg");
@@ -806,8 +809,10 @@ public class FleetCrabWalk : MovementDefinition
var finishSpeed = Math.Min(baseSpeed, Math.Abs(FleetCrabFinishSpeed));
var slowingPow = Math.Max(0.01f, FleetCrabSlowingPow);
var accel = Math.Abs(FleetCrabAccel);
var startAccel = Math.Abs(FleetCrabStartAccel);
var cmdSpeed = 0f;
var lastTick = DateTime.Now;
var speedRampStart = DateTime.Now;
var stopReason = "done";
while (!_stopping)
@@ -837,7 +842,9 @@ public class FleetCrabWalk : MovementDefinition
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;
var rampElapsed = (now - speedRampStart).TotalSeconds;
var activeAccel = rampElapsed < 5.0 ? startAccel : accel;
var speed = activeAccel > 0 ? Slew(cmdSpeed, targetSpeed, activeAccel * dt) : targetSpeed;
cmdSpeed = speed;
var baseCrabTh = (float)CommonMath.ThDiff(phi, cth);
@@ -850,8 +857,8 @@ public class FleetCrabWalk : MovementDefinition
var rawBiasItem = rawBiasMagnitude * biasSign;
var biasItem = ClampAbs(rawBiasItem, conf.BiasThreshold);
var yawSplitSign = GetYawSplitSign(baseCrabTh + biasItem, speed, gcpLimit, controlRadius);
var rawDthItem = conf.DthLinearFac * headingErr * yawSplitSign;
var dthItem = ClampAbs(rawDthItem, conf.DthLinearThreshold);
var rawDthItem = conf.FleetCrabDthLinearFac * headingErr * yawSplitSign;
var dthItem = ClampAbs(rawDthItem, conf.FleetCrabDthLinearThreshold);
var rawFrontTh = baseCrabTh + biasItem + dthItem;
var rawRearTh = baseCrabTh + biasItem - dthItem;
ResolveCrabDriveEquivalent(speed, rawFrontTh, rawRearTh, gcpLimit, out var driveSpeed,
@@ -886,7 +893,7 @@ public class FleetCrabWalk : MovementDefinition
$"ITER#{iter} centerSrc={centerSource} 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} " +
$"slowRatio={slowRatio:0.000} targetV={targetSpeed:0.000} auto=(vx:{driveSpeed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " +
$"slowRatio={slowRatio:0.000} targetV={targetSpeed:0.000} rampT={rampElapsed:0.0} accel={activeAccel:0.000} auto=(vx:{driveSpeed: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");
@@ -895,11 +902,11 @@ public class FleetCrabWalk : MovementDefinition
$"cth:{cth:0.00} crabAngle:{CrabAngleDeg:0.00} bodyToPathCfg:{BodyToPathAngleDeg:0.00} " +
$"targetBodyToPath:{targetBodyToPath:0.00} bodyToPathNow:{baseCrabTh:0.00} " +
$"headingErr(target-current):{headingErr:0.00} reverse(current-target):{headingErrReverse:0.00} yawSign:{yawSplitSign:0} " +
$"dthFac:{conf.DthLinearFac:0.000} rawDth:{rawDthItem:0.00} dth:{dthItem:0.00} dthLimit:{conf.DthLinearThreshold:0.00} " +
$"fleetCrabDthFac:{conf.FleetCrabDthLinearFac:0.000} rawDth:{rawDthItem:0.00} dth:{dthItem:0.00} dthLimit:{conf.FleetCrabDthLinearThreshold:0.00} " +
$"lateral:{lateral:0.0} biasFac:{conf.BiasFac:0.000} biasSign:{biasSign:0} rawBias:{rawBiasItem:0.00} bias:{biasItem:0.00} biasLimit:{conf.BiasThreshold:0.00} " +
$"baseTh:{baseCrabTh:0.00} rawBase:{rawBaseTh:0.00} rawOut(f:{rawFrontTh:0.00},r:{rawRearTh:0.00}) " +
$"out(f:{frontTh:0.00},r:{rearTh:0.00}) gcpLimit:{gcpLimit:0.00} revEq:{reverseEquivalent} " +
$"speedRaw:{speed:0.000} speed:{driveSpeed:0.000} along:{along:0.0} remain:{remain:0.0} ideal=({ideal.X:0.0},{ideal.Y:0.0},{targetBodyTh:0.00})",
$"speedRaw:{speed:0.000} speed:{driveSpeed:0.000} rampT:{rampElapsed:0.0} accel:{activeAccel:0.000} along:{along:0.0} remain:{remain:0.0} ideal=({ideal.X:0.0},{ideal.Y:0.0},{targetBodyTh:0.00})",
"FleetCrabHeadingDbg");
}
yield return true;
@@ -949,6 +956,7 @@ public class FleetCrabWalkTest : MovementTest
CrabLengthMm = PilotDefinition.Conf.FleetCrabLengthMm,
CrabSpeed = PilotDefinition.Conf.FleetCrabSpeed,
FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
FleetCrabStartAccel = PilotDefinition.Conf.FleetCrabStartAccel,
FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,