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");
}
}
}