Add absolute fleet crab walk path support

2026-07-02 17:17:32 +08:00
parent 71444b362d
commit cb104b13cc
2 changed files with 109 additions and 15 deletions
@@ -226,6 +226,65 @@ namespace MultiWheelC
                RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos
            }.Get()).Wait();
        }
+        // 车队联动-自动蟹行：供调度按绝对起终点触发。
+        // CarDirectionBias 表示车队方向相对路径方向的夹角，逆时针为正。
+        public void FleetCrabWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
+            float speed, float CarDirectionBias)
+        {
+            var dx = dstX - srcX;
+            var dy = dstY - srcY;
+            var pathLength = (float)Math.Sqrt(dx * dx + dy * dy);
+            DLog.Log(
+                $"call FleetCrabWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
+                $"len={pathLength:0.0}, speed={speed:0.000}, CarDirectionBias={CarDirectionBias:0.0})",
+                "FleetCrabDbg");
+
+            if (pathLength <= 1f)
+            {
+                DLog.Log("FleetCrabWalk abort: path length is too short.", "FleetCrabDbg");
+                return;
+            }
+
+            if (dstId != -1)
+            {
+                while (!TryLock(dstId))
+                {
+                    Thread.Sleep(50);
+                }
+                DLog.Log($"锁点{dstId}完成", "FleetCrabDbg");
+            }
+
+            var action = new MultiWheelC.FleetCrabWalk
+            {
+                UseAbsolutePath = true,
+                PathStartX = srcX,
+                PathStartY = srcY,
+                PathEndX = dstX,
+                PathEndY = dstY,
+                CrabLengthMm = pathLength,
+                CrabSpeed = speed,
+                BodyToPathAngleDeg = -CarDirectionBias,
+                FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
+                FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
+                FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
+                FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,
+                FleetCrabSlowingPow = PilotDefinition.Conf.FleetCrabSlowingPow,
+                GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
+            };
+
+            try
+            {
+                new DriveTask(action.Get()).Wait();
+            }
+            finally
+            {
+                if (srcId != -1)
+                {
+                    Leave(srcId);
+                    DLog.Log($"释放放车点{srcId}", "FleetCrabDbg");
+                }
+            }
+        }
        public void LineTracking(int srcId, int dstId)
        {
            while (!TryLock(dstId))