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