From d4d2b1a052ae32f4b93e877fdc71deab4e487420 Mon Sep 17 00:00:00 2001
From: "ruifeng.zhou" <ruifeng.zhou@fairylandtech.com>
Date: Thu, 2 Jul 2026 11:04:33 +0800
Subject: [PATCH] fix: keep fleet crab steering angle when stopping

---
 MultiWheel/MultiWheelC/MovementTests.cs | 22 +++++++++++++++-------
 1 file changed, 15 insertions(+), 7 deletions(-)

diff --git a/MultiWheel/MultiWheelC/MovementTests.cs b/MultiWheel/MultiWheelC/MovementTests.cs
index 90e3ed4..bef2980 100644
--- a/MultiWheel/MultiWheelC/MovementTests.cs
+++ b/MultiWheel/MultiWheelC/MovementTests.cs
@@ -568,6 +568,8 @@ public class FleetCrabWalk : MovementDefinition
         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)
@@ -588,10 +590,11 @@ public class FleetCrabWalk : MovementDefinition
                 break;
 
             var targetSpeed = baseSpeed;
+            var slowRatio = 1f;
             if (remain < slowDistance)
             {
-                var ratio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), slowingPow);
-                targetSpeed = 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);
@@ -606,6 +609,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;
 
@@ -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}) " +
                     $"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} " +
-                    $"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} " +
                     $"cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
                     "FleetCrabDbg");
@@ -646,9 +651,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)
         {
@@ -656,8 +664,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;
         }