91 lines
2.7 KiB
C#
91 lines
2.7 KiB
C#
|
|
|
|
using System;
|
|
using ClumsyCore;
|
|
using ClumsyCore.Pilot;
|
|
using FundamentalLib;
|
|
using MDCSToolBox.Clumsy.Movements;
|
|
using MDCSToolBox.Clumsy.Pilot;
|
|
|
|
namespace MultiWheelC
|
|
{
|
|
public abstract class ClampMovementTestBase : MovementTest
|
|
{
|
|
public float TimeoutSeconds = 30f; // 动作超时时间，单位s。
|
|
|
|
private DriveTask _task;
|
|
protected abstract bool Close { get; }
|
|
|
|
// 根据派生测试类型驱动左右夹臂同步夹紧或打开。
|
|
public override void Test()
|
|
{
|
|
var leftTarget = Close
|
|
? PilotDefinition.Self.LeftArmUpperPos
|
|
: PilotDefinition.Self.LeftArmLowerPos;
|
|
var rightTarget = Close
|
|
? PilotDefinition.Self.RightArmUpperPos
|
|
: PilotDefinition.Self.RightArmLowerPos;
|
|
|
|
if (float.IsNaN(leftTarget) ||
|
|
float.IsInfinity(leftTarget) ||
|
|
float.IsNaN(rightTarget) ||
|
|
float.IsInfinity(rightTarget))
|
|
{
|
|
Console.WriteLine(
|
|
"夹臂目标位置无效，取消夹臂运动测试。");
|
|
return;
|
|
}
|
|
|
|
// 防止重复点击时上一项夹臂任务仍在运行。
|
|
TestStop();
|
|
Console.WriteLine(
|
|
$"开始夹臂{(Close ? "夹紧" : "打开")}测试：" +
|
|
$"左目标={leftTarget}，右目标={rightTarget}");
|
|
|
|
var task = new DriveTask(
|
|
new ClampToTarget
|
|
{
|
|
LeftClampTarget = leftTarget,
|
|
RightClampTarget = rightTarget,
|
|
TimeoutSeconds = TimeoutSeconds
|
|
}.Get());
|
|
_task = task;
|
|
|
|
try
|
|
{
|
|
task.Wait();
|
|
}
|
|
finally
|
|
{
|
|
PilotDefinition.Self.SpeedLeftArm = 0f;
|
|
PilotDefinition.Self.SpeedRightArm = 0f;
|
|
if (ReferenceEquals(_task, task))
|
|
_task = null;
|
|
}
|
|
}
|
|
|
|
// 停止夹臂任务并立即清零左右夹臂下发速度。
|
|
public override void TestStop()
|
|
{
|
|
_task?.Stop();
|
|
_task = null;
|
|
PilotDefinition.Self.SpeedLeftArm = 0f;
|
|
PilotDefinition.Self.SpeedRightArm = 0f;
|
|
}
|
|
}
|
|
|
|
[MovementTest(name = "夹臂关闭测试")]
|
|
public sealed class TestClampCloseMovement
|
|
: ClampMovementTestBase
|
|
{
|
|
protected override bool Close => false;
|
|
}
|
|
|
|
[MovementTest(name = "夹臂启动测试")]
|
|
public sealed class TestClampOpenMovement
|
|
: ClampMovementTestBase
|
|
{
|
|
protected override bool Close => true;
|
|
}
|
|
}
|