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

fix: keep fleet crab steering angle when stopping

Browse Source
This commit is contained in:
ruifeng.zhou
2026-07-02 11:04:33 +08:00
parent f127e145e1
commit d4d2b1a052
1 changed files with 15 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheel/MultiWheelC/MovementTests.cs
+15 -7
View File
@@ -568,6 +568,8 @@ public class FleetCrabWalk : MovementDefinition
var cmdSpeed = 0f; var cmdSpeed = 0f;
var lastTick = DateTime.Now; var lastTick = DateTime.Now;
var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold)); var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
var holdFrontTh = ClampAbs((float)CommonMath.ThDiff(phi, theta), gcpLimit);
var holdRearTh = holdFrontTh;
var stopReason = "done"; var stopReason = "done";
while (!_stopping) while (!_stopping)
@@ -588,10 +590,11 @@ public class FleetCrabWalk : MovementDefinition
break; break;
var targetSpeed = baseSpeed; var targetSpeed = baseSpeed;
var slowRatio = 1f;
if (remain < slowDistance) if (remain < slowDistance)
{ {
var ratio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), slowingPow); slowRatio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), slowingPow);
targetSpeed = ratio * (baseSpeed - finishSpeed) + finishSpeed; targetSpeed = slowRatio * (baseSpeed - finishSpeed) + finishSpeed;
} }
var now = DateTime.Now; var now = DateTime.Now;
var dt = Math.Max(0.001f, (float)(now - lastTick).TotalSeconds); var dt = Math.Max(0.001f, (float)(now - lastTick).TotalSeconds);
@@ -606,6 +609,8 @@ public class FleetCrabWalk : MovementDefinition
biasItem = ClampAbs(biasItem, conf.BiasThreshold); biasItem = ClampAbs(biasItem, conf.BiasThreshold);
var frontTh = ClampAbs(baseCrabTh + biasItem + dthItem, gcpLimit); var frontTh = ClampAbs(baseCrabTh + biasItem + dthItem, gcpLimit);
var rearTh = ClampAbs(baseCrabTh + biasItem - dthItem, gcpLimit); var rearTh = ClampAbs(baseCrabTh + biasItem - dthItem, gcpLimit);
holdFrontTh = frontTh;
holdRearTh = rearTh;
var idealAlong = Clamp(along, 0f, CrabLengthMm); var idealAlong = Clamp(along, 0f, CrabLengthMm);
var ideal = new Vector2(x0, y0) + pathDir * idealAlong; var ideal = new Vector2(x0, y0) + pathDir * idealAlong;
@@ -634,7 +639,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}) " + $"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} " + $"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} " + $"baseTh={baseCrabTh:0.0} bias={biasItem:0.0} dth={dthItem:0.0} " +
$"targetV={targetSpeed:0.000} 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} " + $"ideal=({ideal.X:0},{ideal.Y:0},{targetBodyTh:0.0}) scriptEn={self.MultiVehicleScriptEnabled} " +
$"cnt={fleetCnt}/{conf.MultiVehicleFleetNum}", $"cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
"FleetCrabDbg"); "FleetCrabDbg");
@@ -646,9 +651,12 @@ public class FleetCrabWalk : MovementDefinition
stopReason = "stop"; stopReason = "stop";
self.MultiVehicleAutoVx = 0; self.MultiVehicleAutoVx = 0;
self.MultiVehicleAutoFrontTh = 0; self.MultiVehicleAutoFrontTh = holdFrontTh;
self.MultiVehicleAutoRearTh = 0; self.MultiVehicleAutoRearTh = holdRearTh;
self.MultiVehicleAutoCmdTime = DateTime.Now; 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)); var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
while (!_stopping && DateTime.Now < settleEnd) while (!_stopping && DateTime.Now < settleEnd)
{ {
@@ -656,8 +664,8 @@ public class FleetCrabWalk : MovementDefinition
self.MultiVehicleScriptMode = 0; self.MultiVehicleScriptMode = 0;
self.MultiVehicleAutoEnabled = true; self.MultiVehicleAutoEnabled = true;
self.MultiVehicleAutoVx = 0; self.MultiVehicleAutoVx = 0;
self.MultiVehicleAutoFrontTh = 0; self.MultiVehicleAutoFrontTh = holdFrontTh;
self.MultiVehicleAutoRearTh = 0; self.MultiVehicleAutoRearTh = holdRearTh;
self.MultiVehicleAutoCmdTime = DateTime.Now; self.MultiVehicleAutoCmdTime = DateTime.Now;
yield return true; yield return true;
} }