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

update 离车动作连贯

Browse Source
This commit is contained in:
shuai.li
2026-07-04 19:53:09 +08:00
parent e5ef701729
commit 1b4457b341
4 changed files with 82 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheel/MultiWheelC/AGV.cs
+37 -33
View File
@@ -70,20 +70,20 @@ namespace MultiWheelC
}
}
//参数1：tireNum 需要钻过的轮胎对数量
//参数2：frontLidarDetect true:前雷达识别 false:后雷达识别
//参数1：tireNum 需要钻过的轮胎对数量
//参数2：frontLidarDetect true:前雷达识别 false:后雷达识别
public void TireFollowing(int tireNum, bool frontLidarDetect, int srcId, int dstId)
{
while (!TryLock(dstId))
{
Thread.Sleep(50);
}
DLog.Log($"锁点{dstId}完成", "TireFollowing");
var lidarName = frontLidarDetect ? "前雷达" : "后雷达";
DLog.Log($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎", "TireFollowing");
DLog.Log($"锁点{dstId}完成", "TireFollowing");
var lidarName = frontLidarDetect ? "前雷达" : "后雷达";
DLog.Log($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎", "TireFollowing");
if (tireNum != 1 && tireNum != 2)
{
DLog.Log($"TireNum必须是1或2 (当前输入:{tireNum})", "TireFollowing");
DLog.Log($"TireNum必须是1或2 (当前输入:{tireNum})", "TireFollowing");
return;
}
@@ -91,20 +91,20 @@ namespace MultiWheelC
{
while (!TryLock(dstId))
{
Console.WriteLine("等待锁取货点中...");
Console.WriteLine("等待锁取货点中...");
Thread.Sleep(200);
}
Console.WriteLine($"锁点{dstId}完成");
Console.WriteLine($"锁点{dstId}完成");
Thread.Sleep(1000);
Console.WriteLine($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎");
Console.WriteLine($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎");
Thread.Sleep(1000);
Leave(srcId);
Console.WriteLine($"开始第一段盲走，此时释放预取货点{srcId}");
Console.WriteLine($"开始第一段盲走，此时释放预取货点{srcId}");
Thread.Sleep(2000);
//Leave(dstId);
//Console.WriteLine($"结束第一段盲走，此时释放取货点{dstId}");
//Console.WriteLine($"结束第一段盲走，此时释放取货点{dstId}");
Thread.Sleep(2000);
Console.WriteLine($"结束钻车动作");
Console.WriteLine($"结束钻车动作");
return;
}
@@ -138,7 +138,7 @@ namespace MultiWheelC
0)
},
};
DLog.Log($"钻胎为{tireNum}", "TireFollowing");
DLog.Log($"钻胎为{tireNum}", "TireFollowing");
var following = new TireFollowing()
{
GetController = () => new ChassisController().Get(),
@@ -153,18 +153,18 @@ namespace MultiWheelC
};
var _dt = new DriveTask(following.Get());
_dt.Wait();
DLog.Log("钻车动作结束", "TireFollowing");
DLog.Log("钻车动作结束", "TireFollowing");
}
//离车一定是后雷达识别一个轮胎
//离车一定是后雷达识别一个轮胎
public void LeaveCar(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
{
while (!TryLock(dstId))
{
Thread.Sleep(50);
}
DLog.Log($"锁点{dstId}完成", "TireFollowing");
DLog.Log($"开始钻车动作，通过后雷达识别结果钻1对轮胎", "TireFollowing");
DLog.Log($"锁点{dstId}完成", "TireFollowing");
DLog.Log($"开始钻车动作，通过后雷达识别结果钻1对轮胎", "TireFollowing");
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
chassis.SetOriginBias(0, 0, 0);
var following = new TireFollowing()
@@ -193,6 +193,9 @@ namespace MultiWheelC
CarDirection = 180f,
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
EnableHandover = true,
HandoverDistance = 200f,
HandoverSpeed = 0.3f,
WalkBlindTh = 0,
TireNum = 1
};
@@ -204,14 +207,15 @@ namespace MultiWheelC
if (!running) break;
yield return true;
}
DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
DLog.Log("离车TireFollowing结束，开始DstTracker", "TireFollowing");
DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
DLog.Log("离车TireFollowing结束，开始DstTracker", "TireFollowing");
foreach (var running in new DstTracker()
{
Src = new Vector2(srcX, srcY),
Dst = new Vector2(dstX, dstY),
CarDirectionBias = 180f,
InitialSendSpeed = 0.3f
}.Get())
{
if (!running) break;
@@ -223,7 +227,7 @@ namespace MultiWheelC
var _dt = new DriveTask(LeaveThenFollow());
_dt.Wait();
DLog.Log("离车动作1结束", "TireFollowing");
DLog.Log("离车动作1结束", "TireFollowing");
}
public void LineTracking(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
@@ -234,7 +238,7 @@ namespace MultiWheelC
}
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
chassis.SetOriginBias(0, 0, 0);
DLog.Log($"锁点{dstId}完成", "TireFollowing");
DLog.Log($"锁点{dstId}完成", "TireFollowing");
IEnumerable<bool> TrackThenFollow()
{
foreach (var running in new LineTracking()
@@ -247,11 +251,13 @@ namespace MultiWheelC
if (!running) break;
yield return true;
}
DLog.Log("离车LineTracking结束，开始DstTracker", "TireFollowing");
DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
DLog.Log("离车LineTracking结束，开始DstTracker", "TireFollowing");
foreach (var running in new DstTracker()
{
Src = new Vector2(srcX, srcY),
Dst = new Vector2(dstX, dstY),
InitialSendSpeed = 0.3f
}.Get())
{
if (!running) break;
@@ -261,34 +267,32 @@ namespace MultiWheelC
}
var _dt = new DriveTask(TrackThenFollow());
_dt.Wait();
//Leave(srcId);
DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
DLog.Log("离车动作2结束", "TireFollowing");
DLog.Log("离车动作2结束", "TireFollowing");
}
//驱动器上使能
//驱动器上使能
public void DriverAble()
{
var dl = new DriveTask(new DriverAble() { }.Get());
dl.Wait();
DLog.Log("驱动器上使能完成", "TireFollowing");
DLog.Log("驱动器上使能完成", "TireFollowing");
}
//驱动器下使能
//驱动器下使能
public void DriverDisable()
{
var dl = new DriveTask(new DriverDisable() { }.Get());
dl.Wait();
DLog.Log("驱动器下使能完成", "TireFollowing");
DLog.Log("驱动器下使能完成", "TireFollowing");
}
// 夹抱：close 为 true 时关闭夹抱，否则打开夹抱。
// 夹抱：close 为 true 时关闭夹抱，否则打开夹抱。
public void ClamptoTarget(bool close)
{
if (PilotDefinition.Self.GhostMode)
{
Thread.Sleep(2000);
Console.WriteLine("夹抱完成");
Console.WriteLine("夹抱完成");
return;
}
new DriveTask(new ClampToTarget()
@@ -334,7 +338,7 @@ namespace MultiWheelC
{
Thread.Sleep(50);
}
DLog.Log($"锁点{dstId}完成", "FleetCrabDbg");
DLog.Log($"锁点{dstId}完成", "FleetCrabDbg");
}
var action = new MultiWheelC.FleetCrabWalk
@@ -360,7 +364,7 @@ namespace MultiWheelC
if (srcId != -1)
{
Leave(srcId);
DLog.Log($"释放放车点{srcId}", "FleetCrabDbg");
DLog.Log($"释放放车点{srcId}", "FleetCrabDbg");
}
}
}
MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs
+4
View File
@@ -385,6 +385,9 @@ namespace MultiWheelC
foreach (var running in new LineTracking()
{
Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
EnableHandover = true,
HandoverDistance = 200f,
HandoverSpeed = 0.3f,
}.Get())
{
if (!running) break;
@@ -395,6 +398,7 @@ namespace MultiWheelC
Src = src,
Dst = dst,
CarDirectionBias = carDirectionBias,
InitialSendSpeed = 0.3f
}.Get())
{
if (!running) break;
MultiWheel/MultiWheelC/Movements.cs
+26 -1
View File
@@ -176,15 +176,23 @@ namespace MultiWheelC
public float CarDirectionBias = 0f;
public Painter Painter = UI.GetPainter("DstTracker");
public float InitialSendSpeed = 0;
public override IEnumerable<bool> Get()
{
Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
var tracker = new ChassisController().Get();
if (InitialSendSpeed != 0)
{
tracker.SkipInitialRotate = true;
tracker.InitialSendSpeed = InitialSendSpeed;
}
var linePath = new LineTrack(Src, Dst) { CarDirectionBias = CarDirectionBias, Speed = PilotDefinition.Conf.DstTrackerMaxSpeed };
tracker.AddTrack(linePath);
var task = new DriveTask(tracker.Track());
task.Wait();
@@ -208,6 +216,10 @@ namespace MultiWheelC
public int DstId = -1;
public Action<int> LeaveSrcFunction = null;
private PIDController pid;
// 末段衔接：接近目标后不再让 PID 把速度降到 0，保留一个接力速度给后续动作接管
public bool EnableHandover = false;
public float HandoverDistance = 80f; // mm
public float HandoverSpeed = 0.15f; // m/s
public override IEnumerable<bool> Get()
{
@@ -222,6 +234,19 @@ namespace MultiWheelC
DLog.Log($"直线行驶距离:{Target}", "TireFollowing");
while (true)
{
var current = (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2;
var remain = Target - current;
if (EnableHandover && Math.Abs(remain) <= Math.Max(1f, HandoverDistance))
{
var handoverSign = Math.Sign(remain);
if (handoverSign == 0) handoverSign = 1;
var handoverSpeed = Math.Abs(HandoverSpeed) * handoverSign;
Console.WriteLine($"handover speed: {handoverSpeed:F3}, remain: {remain:F2}");
chassis.SendXYThSpeed(handoverSpeed, 0, 0);
// 保留一拍接力速度，让后续 DstTracker 无缝接管
yield return true;
break;
}
var speed = pid.GetResponse(Target);
Console.WriteLine($"output: {speed} current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}");
MultiWheel/MultiWheelC/TireFollowing.cs
+15 -2
View File
@@ -50,6 +50,11 @@ namespace MultiWheelC
/// </summary>
public float MaxSpeed = 0.3f;
// 末段衔接：接近盲走终点时给非零速度，供后续动作连续接管
public bool EnableHandover = false;
public float HandoverDistance = 200f; // mm
public float HandoverSpeed = 0.2f; // m/s
/// <summary>
/// 钻轮胎数量
/// </summary>
@@ -289,6 +294,11 @@ namespace MultiWheelC
controller.SlowDistance = SlowDistance;
controller.SlowingPow = 0.7f;
}
if (EnableHandover)
{
controller.SlowDistance = float.MinValue;
controller.FinishDistance = 20;
}
(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh) = GetCurrentPos2Dst(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
var walkBlindPathEnd = Tuple.Create(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
var walkBlindPathStart = LessMath.Transform2D(walkBlindPathEnd, Tuple.Create(CarDirection == 0 ? -3000f : 3000f, 0f, 0f));
@@ -324,7 +334,7 @@ namespace MultiWheelC
_remainAngleList.Clear();
_remainDistanceList.Clear();
detectorIndex++;
if (detectors.Count == 1 || TireNum == 1)
if ((detectors.Count == 1 || TireNum == 1) && !EnableHandover)
{
HardStop();
yield return false;
@@ -333,7 +343,10 @@ namespace MultiWheelC
else if (WalkBlindStage2)
{
DLog.Log("达到第二对轮胎处，停止移动", "TireFollowing");
HardStop();
if (!EnableHandover)
{
HardStop();
}
yield return false;
}
}