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-02 17:52:55 +08:00
parent 71444b362d
commit d3b4a9eb15
4 changed files with 165 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheel/MultiWheelC/AGV.cs
+12 -3
View File
@@ -226,7 +226,7 @@ namespace MultiWheelC
RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos
}.Get()).Wait();
}
public void LineTracking(int srcId, int dstId)
public void LineTracking(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
{
while (!TryLock(dstId))
{
@@ -236,9 +236,18 @@ namespace MultiWheelC
new DriveTask(new LineTracking()
{
Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
LeaveSrcFunction = Leave,
SrcId = srcId,
//LeaveSrcFunction = Leave,
//SrcId = srcId,
}.Get()).Wait();
new DriveTask(new DstTracker()
{
Src = new Vector2(srcX, srcY),
Dst = new Vector2(dstX, dstY),
}.Get());Wait();
Leave(srcId);
DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
}
public void ChangeAvoidanceDistance(float stopDistance, float slowDistance)
MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs
+124 -1
View File
@@ -292,7 +292,130 @@ namespace MultiWheelC
private DriveTask _dt;
}
[MovementTest(name = "驱动器下使能测试")]
[MovementTest(name = "测试终点跟踪动作-前进")]
public class DstTrackerForward : MovementTest
{
public bool UseInteractivePick = true;
public float srcX;
public float srcY;
public float dstX;
public float dstY;
public float carDirectionBias = 0f;
private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
public override void TestStop()
{
_dt?.Stop();
_painter?.Clear();
}
public override void Test()
{
var p1 = UI.GetPoint("point1");
var p2 = UI.GetPoint("point2");
_painter.Clear();
_dt = new DriveTask(new DstTracker()
{
Src = p1,
Dst = p2,
CarDirectionBias = carDirectionBias,
}.Get());
_dt.Wait();
}
private DriveTask _dt;
}
[MovementTest(name = "测试终点跟踪动作-后退")]
public class DstTrackerhoutui : MovementTest
{
public bool UseInteractivePick = true;
public float srcX;
public float srcY;
public float dstX;
public float dstY;
public float carDirectionBias = 180f;
private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
public override void TestStop()
{
_dt?.Stop();
_painter?.Clear();
}
public override void Test()
{
var p1 = UI.GetPoint("point1");
var p2 = UI.GetPoint("point2");
_painter.Clear();
_dt = new DriveTask(new DstTracker()
{
Src = p1,
Dst = p2,
CarDirectionBias = carDirectionBias,
}.Get());
_dt.Wait();
}
private DriveTask _dt;
}
[MovementTest(name = "测试先直行再终点跟踪")]
public class LineTrackThenDstTrackerTest : MovementTest
{
public float carDirectionBias = 0f;
private readonly Painter _painter = UI.GetPainter("LineTrackThenDstTrackerTest");
public override void TestStop()
{
_dt?.Stop();
_painter?.Clear();
}
public override void Test()
{
var src = UI.GetPoint("请在上位机选择起点(src)");
var dst = UI.GetPoint("请在上位机选择终点(dst)");
_painter.Clear();
_painter.DrawLine(Color.Cyan, src.X, src.Y, dst.X, dst.Y, width: 3);
_painter.DrawCircle(Color.LimeGreen, src.X, src.Y, 80f);
_painter.DrawCircle(Color.OrangeRed, dst.X, dst.Y, 80f);
_painter.DrawText(Color.LimeGreen, "src", src.X + 80f, src.Y + 80f);
_painter.DrawText(Color.OrangeRed, "dst", dst.X + 80f, dst.Y + 80f);
IEnumerable<bool> TrackThenFollow()
{
foreach (var running in new LineTracking()
{
Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
}.Get())
{
if (!running) break;
yield return true;
}
// 衔接速度：避免第一段结束后瞬间“掉速”，让第二段接管更连贯
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
var handoverSpeed = Math.Max(0.08f, Math.Min(PilotDefinition.Conf.DstTrackerMaxSpeed, 0.2f));
var direction = Math.Sign(dst.x - src.x);
if (direction == 0) direction = 1;
chassis.SendXYThSpeed(handoverSpeed * direction, 0f, 0f);
yield return true;
foreach (var running in new DstTracker()
{
Src = src,
Dst = dst,
CarDirectionBias = carDirectionBias,
}.Get())
{
if (!running) break;
yield return true;
}
yield return false;
}
_dt = new DriveTask(TrackThenFollow());
_dt.Wait();
}
private DriveTask _dt;
}
public class DriverDisableTest : MovementTest
{
public override void TestStop()
MultiWheel/MultiWheelC/Movements.cs
+27
View File
@@ -167,6 +167,33 @@ namespace MultiWheelC
}
}
//在世界坐标系下，从路径起点追踪到终点并停车
public class DstTracker : MovementDefinition
{
public Vector2 Src;
public Vector2 Dst;
public float CarDirectionBias = 0f;
public Painter Painter = UI.GetPainter("DstTracker");
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();
var linePath = new LineTrack(Src, Dst) { CarDirectionBias = CarDirectionBias, Speed = PilotDefinition.Conf.DstTrackerMaxSpeed };
tracker.AddTrack(linePath);
var task = new DriveTask(tracker.Track());
task.Wait();
// 到点后兜底停车
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
chassis.SendXYThSpeed(0f, 0f, 0f);
yield return false;
}
}
//直线行走基于轮里程
public class LineTracking : MovementDefinition
{
MultiWheel/MultiWheelC/PilotConfig.cs
+2
View File
@@ -287,5 +287,7 @@ public class PilotConfig : MultiWheelPilotConfig
[FieldMember(desc = "轮胎跟踪:距离过近角度忽略阈值")] public float TireFollowingAngleIgnoreThr = 0.2f;
[FieldMember(desc = "轮胎跟踪:Y最大平均数")] public int TireFollowingYAverageFrameCount = 5;
[FieldMember(desc = "终点跟踪:速度")] public float DstTrackerMaxSpeed = 0.3f;
#endregion
}