93 lines
2.9 KiB
C#
93 lines
2.9 KiB
C#
|
||
|
||
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using ClumsyCore.Pilot;
|
||
using CommonUsage.Chassis;
|
||
using MyParking.Shared;
|
||
|
||
namespace MultiWheelC
|
||
{
|
||
// C层测试准备：停车并等待四个舵轮稳定回到车体前向0°。
|
||
public class PrepareWheelsForward : MovementDefinition
|
||
{
|
||
public float ToleranceDegrees = 2f;
|
||
public float StableSeconds = 0.3f;
|
||
public float TimeoutSeconds = 10f;
|
||
public bool Completed { get; private set; }
|
||
|
||
public override IEnumerable<bool> Get()
|
||
{
|
||
var chassis =
|
||
PilotDefinition.Chassis as MultiWheelChassis;
|
||
if (chassis == null)
|
||
{
|
||
throw new InvalidOperationException(
|
||
"当前底盘不是MultiWheelChassis，无法执行舵轮回正。");
|
||
}
|
||
|
||
var adapter = new MultiWheelChassisAdapter(
|
||
chassis,
|
||
PilotDefinition.Self.CarNum);
|
||
adapter.ResetToBodyFrame();
|
||
var toleranceRadians =
|
||
AngleMath.DegreesToRadians(ToleranceDegrees);
|
||
var startTime = DateTime.UtcNow;
|
||
DateTime? alignedSince = null;
|
||
|
||
Completed = false;
|
||
if (!adapter.PrepareParallelDirection(0.0))
|
||
{
|
||
throw new InvalidOperationException(
|
||
"无法将所有舵轮下发到车体前向0°。");
|
||
}
|
||
|
||
try
|
||
{
|
||
while (true)
|
||
{
|
||
var aligned =
|
||
adapter.AreParallelWheelsAligned(
|
||
0.0,
|
||
toleranceRadians);
|
||
|
||
if (aligned)
|
||
{
|
||
if (!alignedSince.HasValue)
|
||
alignedSince = DateTime.UtcNow;
|
||
|
||
if ((DateTime.UtcNow -
|
||
alignedSince.Value).TotalSeconds >=
|
||
StableSeconds)
|
||
{
|
||
Completed = true;
|
||
yield break;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
alignedSince = null;
|
||
}
|
||
|
||
if (TimeoutSeconds > 0f &&
|
||
(DateTime.UtcNow - startTime).TotalSeconds >
|
||
TimeoutSeconds)
|
||
{
|
||
throw new TimeoutException(
|
||
$"舵轮回正超过{TimeoutSeconds:F1}s，" +
|
||
"测试已经取消。");
|
||
}
|
||
|
||
yield return true;
|
||
}
|
||
}
|
||
finally
|
||
{
|
||
// 只清零驱动速度，保留已经下发的0°舵角。
|
||
adapter.StopImmediately();
|
||
}
|
||
}
|
||
}
|
||
}
|