141 lines
4.6 KiB
C#
141 lines
4.6 KiB
C#
|
||
|
||
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using ClumsyCore.Pilot;
|
||
using CommonUsage.Chassis;
|
||
using MyParking.Shared;
|
||
|
||
namespace MultiWheelC
|
||
{
|
||
/// <summary>
|
||
/// 停车并等待四个舵轮稳定回到车体前向0°。
|
||
/// </summary>
|
||
public class PrepareWheelsForward : MovementDefinition
|
||
{
|
||
/// <summary>
|
||
/// 获取或设置舵轮需要对准的车体方向，单位为rad；0表示车头方向。
|
||
/// </summary>
|
||
public double DirectionRadians;
|
||
|
||
/// <summary>
|
||
/// 获取或设置本次动作的回正到位容差覆盖值，单位为deg；为空时读取车辆配置。
|
||
/// </summary>
|
||
public float? ToleranceDegrees;
|
||
|
||
/// <summary>
|
||
/// 获取或设置本次动作的稳定确认时间覆盖值，单位为s；为空时读取车辆配置。
|
||
/// </summary>
|
||
public float? StableSeconds;
|
||
|
||
/// <summary>
|
||
/// 获取或设置本次动作的超时覆盖值，单位为s；为空时读取车辆配置，0表示关闭超时。
|
||
/// </summary>
|
||
public float? TimeoutSeconds;
|
||
|
||
public bool Completed { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 读取一次有效配置并等待全部舵轮在容差内稳定保持车体前向0°。
|
||
/// </summary>
|
||
public override IEnumerable<bool> Get()
|
||
{
|
||
NumericGuard.EnsureFinite(
|
||
DirectionRadians,
|
||
nameof(DirectionRadians));
|
||
|
||
var config = PilotDefinition.Conf;
|
||
var toleranceDegrees =
|
||
ToleranceDegrees ??
|
||
config.ParkingWheelForwardToleranceDegrees;
|
||
var stableSeconds =
|
||
StableSeconds ??
|
||
config.ParkingWheelForwardStableSeconds;
|
||
var timeoutSeconds =
|
||
TimeoutSeconds ??
|
||
config.ParkingWheelForwardTimeoutSeconds;
|
||
|
||
NumericGuard.EnsureFiniteNonNegative(
|
||
toleranceDegrees,
|
||
nameof(ToleranceDegrees));
|
||
NumericGuard.EnsureFiniteNonNegative(
|
||
stableSeconds,
|
||
nameof(StableSeconds));
|
||
NumericGuard.EnsureFiniteNonNegative(
|
||
timeoutSeconds,
|
||
nameof(TimeoutSeconds));
|
||
|
||
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(
|
||
DirectionRadians))
|
||
{
|
||
throw new InvalidOperationException(
|
||
"无法将所有舵轮下发到指定运动方向。");
|
||
}
|
||
|
||
try
|
||
{
|
||
while (true)
|
||
{
|
||
var aligned =
|
||
adapter.AreParallelWheelsAligned(
|
||
DirectionRadians,
|
||
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
|
||
{
|
||
// 只清零驱动速度，保留已经下发的目标舵角。
|
||
adapter.StopImmediately();
|
||
}
|
||
}
|
||
}
|
||
}
|