102 lines
3.0 KiB
C#
102 lines
3.0 KiB
C#
using ClumsyCore;
|
|
using ClumsyCore.DTools;
|
|
using ClumsyCore.Interfaces;
|
|
using ClumsyCore.Pilot;
|
|
using MDCSToolBox.Commons.Controllers;
|
|
using System;
|
|
using System.Numerics;
|
|
|
|
namespace MultiWheelC
|
|
{
|
|
public abstract class DstTrackerTestBase : MovementTest
|
|
{
|
|
public bool UseInteractivePick = true;
|
|
public float srcX;
|
|
public float srcY;
|
|
public float dstX;
|
|
public float dstY;
|
|
public float carDirectionBias;
|
|
|
|
private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
|
|
private DriveTask _dt;
|
|
|
|
protected DstTrackerTestBase(float defaultCarDirectionBias)
|
|
{
|
|
carDirectionBias = defaultCarDirectionBias;
|
|
}
|
|
|
|
public override void TestStop()
|
|
{
|
|
_dt?.Stop();
|
|
_painter?.Clear();
|
|
}
|
|
|
|
public override void Test()
|
|
{
|
|
Vector2 p1;
|
|
Vector2 p2;
|
|
if (UseInteractivePick)
|
|
{
|
|
p1 = UI.GetPoint("point1");
|
|
p2 = UI.GetPoint("point2");
|
|
}
|
|
else
|
|
{
|
|
p1 = new Vector2(srcX, srcY);
|
|
p2 = new Vector2(dstX, dstY);
|
|
}
|
|
|
|
_painter.Clear();
|
|
_dt = new DriveTask(new DstTracker
|
|
{
|
|
Src = p1,
|
|
Dst = p2,
|
|
CarDirectionBias = carDirectionBias,
|
|
}.Get());
|
|
_dt.Wait();
|
|
}
|
|
}
|
|
|
|
[MovementTest(name = "测试终点跟踪动作-前进")]
|
|
public sealed class DstTrackerForward : DstTrackerTestBase
|
|
{
|
|
public DstTrackerForward() : base(0f) { }
|
|
}
|
|
|
|
[MovementTest(name = "测试终点跟踪动作-后退")]
|
|
public sealed class DstTrackerBackward : DstTrackerTestBase
|
|
{
|
|
public DstTrackerBackward() : base(180f) { }
|
|
}
|
|
|
|
[MovementTest(name = "底盘旋转测试")]
|
|
public class RotateToAngleTest : MovementTest
|
|
{
|
|
// 底盘旋转测试不支持停止操作。
|
|
public override void TestStop()
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
// 交互输入目标角度后执行底盘原地旋转测试。
|
|
public override void Test()
|
|
{
|
|
var target = UI.GetInput("输入旋转角度：");
|
|
new DriveTask(new MultiWheelRotateInPlace()
|
|
{
|
|
AngleTarget = float.Parse(target),
|
|
PidparamsRead = () => new PIDParams()
|
|
{
|
|
Kp = PilotDefinition.Conf.InPlaceRotateKp,
|
|
Ki = PilotDefinition.Conf.InPlaceRotateKi,
|
|
Kd = PilotDefinition.Conf.InPlaceRotateKd,
|
|
DeadZone = PilotDefinition.Conf.InPlaceRotateArriveDeg,
|
|
SpeedAccPerSec = PilotDefinition.Conf.InPlaceRotateAcc,
|
|
OutputUpperThreshold = PilotDefinition.Conf.InPlaceRotateMaxSpeed,
|
|
MaxI = PilotDefinition.Conf.InPlaceRotateMaxI,
|
|
}
|
|
}.Get()).Wait();
|
|
}
|
|
}
|
|
}
|