diff --git a/MultiWheel/MultiWheelC/AGV.cs b/MultiWheel/MultiWheelC/AGV.cs
index 169899f..33e3904 100644
--- a/MultiWheel/MultiWheelC/AGV.cs
+++ b/MultiWheel/MultiWheelC/AGV.cs
@@ -226,7 +226,66 @@ namespace MultiWheelC
RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos
}.Get()).Wait();
}
- public void LineTracking(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
+ // 车队联动-自动蟹行：供调度按绝对起终点触发。
+ // 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))
{
diff --git a/MultiWheel/MultiWheelC/MovementTests.cs b/MultiWheel/MultiWheelC/MovementTests.cs
index 34c0f26..f0a819a 100644
--- a/MultiWheel/MultiWheelC/MovementTests.cs
+++ b/MultiWheel/MultiWheelC/MovementTests.cs
@@ -447,6 +447,14 @@ public class FleetCrabWalk : MovementDefinition
/// 璺緞闀垮害(mm)銆
public float CrabLengthMm = 2000f;
+ /// 浣跨敤璋冨害缁欏畾鐨勭粷瀵硅捣缁堢偣璺緞锛岃屼笉鏄敤褰撳墠杞﹂槦涓績鍜 CrabAngleDeg 鐢熸垚璺緞銆
+ public bool UseAbsolutePath = false;
+
+ public float PathStartX;
+ public float PathStartY;
+ public float PathEndX;
+ public float PathEndY;
+
/// 琛岄┒閫熷害(m/s)銆
public float CrabSpeed = 0.2f;
@@ -525,8 +533,9 @@ public class FleetCrabWalk : MovementDefinition
$"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " +
$"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " +
$"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " +
- $"autoFields=true pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
- $"gcpLimit={GcpThetaThreshold:0.0} biasFac={conf.BiasFac:0.00} dthFac={conf.DthLinearFac:0.00}",
+ $"autoFields=true pathMode={(UseAbsolutePath ? "absolute" : "relative")} pathAngle={CrabAngleDeg:0.0} " +
+ $"bodyToPath={BodyToPathAngleDeg:0.0} gcpLimit={GcpThetaThreshold:0.0} " +
+ $"biasFac={conf.BiasFac:0.00} dthFac={conf.DthLinearFac:0.00}",
"FleetCrabDbg");
if (conf.MultiVehicleMasterEndpoint != "/")
@@ -546,17 +555,43 @@ public class FleetCrabWalk : MovementDefinition
}
DLog.Log($"CENTER 杞﹂槦涓績=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
- var phi = CommonMath.RoundTh(theta + CrabAngleDeg);
+ Vector2 pathStart;
+ Vector2 dst;
+ float pathLengthMm;
+ float phi;
+ if (UseAbsolutePath)
+ {
+ pathStart = new Vector2(PathStartX, PathStartY);
+ dst = new Vector2(PathEndX, PathEndY);
+ var pathVec = dst - pathStart;
+ pathLengthMm = pathVec.Length();
+ if (pathLengthMm <= 1f)
+ {
+ DLog.Log(
+ $"ABORT: absolute path is too short src=({PathStartX:0},{PathStartY:0}) dst=({PathEndX:0},{PathEndY:0}) len={pathLengthMm:0.0}",
+ "FleetCrabDbg");
+ Hedingben.ToastText("杞﹂槦锜硅璺緞闀垮害杩囩煭", "FleetCrab");
+ yield break;
+ }
+ phi = CommonMath.RoundTh((float)(Math.Atan2(pathVec.Y, pathVec.X) / Math.PI * 180.0));
+ }
+ else
+ {
+ pathStart = new Vector2(x0, y0);
+ pathLengthMm = CrabLengthMm;
+ phi = CommonMath.RoundTh(theta + CrabAngleDeg);
+ dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
+ }
var targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
- var dst = CommonMath.Transform2D(new Vector2(x0, y0), phi, new Vector2(CrabLengthMm, 0));
var phiRad = phi / 180.0 * Math.PI;
var pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
var pathLeft = new Vector2(-pathDir.Y, pathDir.X);
DLog.Log(
- $"START center=({x0:0},{y0:0},{theta:0.0}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
+ $"START center=({x0:0},{y0:0},{theta:0.0}) pathMode={(UseAbsolutePath ? "absolute" : "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={CrabLengthMm: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} accel={FleetCrabAccel:0.000} " +
$"slow={FleetCrabSlowDistance:0} finishDist={FleetCrabFinishDistance:0} " +
$"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}",
"FleetCrabDbg");
@@ -570,8 +605,8 @@ public class FleetCrabWalk : MovementDefinition
self.MultiVehicleAutoVx = 0;
self.MultiVehicleAutoFrontTh = 0;
self.MultiVehicleAutoRearTh = 0;
- self.MultiVehicleAutoIdealX = x0;
- self.MultiVehicleAutoIdealY = y0;
+ self.MultiVehicleAutoIdealX = pathStart.X;
+ self.MultiVehicleAutoIdealY = pathStart.Y;
self.MultiVehicleAutoIdealTh = targetBodyTh;
self.MultiVehicleAutoHasIdeal = true;
self.MultiVehicleAutoCmdTime = DateTime.Now;
@@ -590,8 +625,8 @@ public class FleetCrabWalk : MovementDefinition
self.MultiVehicleAutoVx = 0;
self.MultiVehicleAutoFrontTh = 0;
self.MultiVehicleAutoRearTh = 0;
- self.MultiVehicleAutoIdealX = x0;
- self.MultiVehicleAutoIdealY = y0;
+ self.MultiVehicleAutoIdealX = pathStart.X;
+ self.MultiVehicleAutoIdealY = pathStart.Y;
self.MultiVehicleAutoIdealTh = targetBodyTh;
self.MultiVehicleAutoHasIdeal = true;
self.MultiVehicleAutoCmdTime = DateTime.Now;
@@ -618,7 +653,7 @@ public class FleetCrabWalk : MovementDefinition
DLog.Log("WARMUP timeout: fleet members are not ready; continue with auto fields and safety interlock.",
"FleetCrabDbg");
- Hedingben.ToastText($"杞﹂槦锜硅 璺緞{CrabAngleDeg:0.0}掳 杞﹁韩澶硅{BodyToPathAngleDeg:0.0}掳 闀垮害{CrabLengthMm:0}mm", "FleetCrab");
+ Hedingben.ToastText($"杞﹂槦锜硅 璺緞{phi:0.0}掳 杞﹁韩澶硅{BodyToPathAngleDeg:0.0}掳 闀垮害{pathLengthMm:0}mm", "FleetCrab");
var iter = 0;
var lastLog = DateTime.MinValue;
@@ -645,10 +680,10 @@ public class FleetCrabWalk : MovementDefinition
DLog.Log("ABORT: TryGetFleetCenterFromSlam returned false during auto crab.", "FleetCrabDbg");
break;
}
- var delta = new Vector2(cx - x0, cy - y0);
+ var delta = new Vector2(cx - pathStart.X, cy - pathStart.Y);
var along = Vector2.Dot(delta, pathDir);
var lateral = Vector2.Dot(delta, pathLeft);
- var remain = CrabLengthMm - along;
+ var remain = pathLengthMm - along;
if (remain <= finishDistance)
break;
@@ -674,8 +709,8 @@ public class FleetCrabWalk : MovementDefinition
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;
+ var idealAlong = Clamp(along, 0f, pathLengthMm);
+ var ideal = pathStart + pathDir * idealAlong;
self.MultiVehicleScriptEnabled = false;
self.MultiVehicleScriptMode = 0;