56 lines
1.8 KiB
C#
56 lines
1.8 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Drawing;
|
|
using System.Numerics;
|
|
using ClumsyCore;
|
|
using ClumsyCore.DTools;
|
|
using ClumsyCore.Pilot;
|
|
using CommonUsage.Chassis;
|
|
using MDCSToolBox.Clumsy.Tracks;
|
|
|
|
namespace MultiWheelC
|
|
{
|
|
//在世界坐标系下，从路径起点追踪到终点并停车
|
|
public class DstTracker : MovementDefinition
|
|
{
|
|
public Vector2 Src;
|
|
public Vector2 Dst;
|
|
// 本次轨迹的巡航速度上限，单位m/s。
|
|
public float MaxSpeed = PilotDefinition.Conf.DstTrackerMaxSpeed;
|
|
public float CarDirectionBias = 0f;
|
|
public Painter Painter = UI.GetPainter("DstTracker");
|
|
public override IEnumerable<bool> Get()
|
|
{
|
|
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
|
DriveTask task = null;
|
|
try
|
|
{
|
|
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
|
|
{
|
|
BaseSpeed = MaxSpeed
|
|
}.Get();
|
|
// 要求路径末端速度下降到零。
|
|
tracker.FinishSpeed = 0f;
|
|
var linePath = new LineTrack(Src, Dst)
|
|
{
|
|
CarDirectionBias = CarDirectionBias,
|
|
Speed = MaxSpeed
|
|
};
|
|
tracker.AddTrack(linePath);
|
|
task = new DriveTask(tracker.Track());
|
|
task.Wait();
|
|
yield return false;
|
|
}
|
|
finally
|
|
{
|
|
task?.Stop();
|
|
chassis.PredefinedDriveStop();
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|