77 lines
2.4 KiB
C#
77 lines
2.4 KiB
C#
using System;
|
|
|
|
namespace MultiWheelC.Control.Abstractions
|
|
{
|
|
/// <summary>
|
|
/// 表示横向控制器生成的前、后GCP目标转角，单位为rad，逆时针为正。
|
|
/// </summary>
|
|
public readonly struct LateralControlCommand
|
|
{
|
|
/// <summary>
|
|
/// 创建前、后GCP目标转角命令。
|
|
/// </summary>
|
|
public LateralControlCommand(
|
|
double frontGcpAngleRadians,
|
|
double rearGcpAngleRadians)
|
|
{
|
|
EnsureFinite(
|
|
frontGcpAngleRadians,
|
|
nameof(frontGcpAngleRadians));
|
|
EnsureFinite(
|
|
rearGcpAngleRadians,
|
|
nameof(rearGcpAngleRadians));
|
|
|
|
FrontGcpAngleRadians =
|
|
frontGcpAngleRadians;
|
|
RearGcpAngleRadians =
|
|
rearGcpAngleRadians;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取前GCP目标转角，单位为rad，逆时针为正。
|
|
/// </summary>
|
|
public double FrontGcpAngleRadians { get; }
|
|
|
|
/// <summary>
|
|
/// 获取后GCP目标转角，单位为rad，逆时针为正。
|
|
/// </summary>
|
|
public double RearGcpAngleRadians { get; }
|
|
|
|
/// <summary>
|
|
/// 获取前后GCP的共同转角分量，主要用于横向平移修正。
|
|
/// </summary>
|
|
public double CommonAngleRadians =>
|
|
(FrontGcpAngleRadians +
|
|
RearGcpAngleRadians) / 2.0;
|
|
|
|
/// <summary>
|
|
/// 获取前后GCP的差动转角分量，主要用于曲率前馈和航向修正。
|
|
/// </summary>
|
|
public double DifferentialAngleRadians =>
|
|
(FrontGcpAngleRadians -
|
|
RearGcpAngleRadians) / 2.0;
|
|
|
|
/// <summary>
|
|
/// 创建前后GCP均保持车头方向的直线命令。
|
|
/// </summary>
|
|
public static LateralControlCommand Straight =>
|
|
new LateralControlCommand(0.0, 0.0);
|
|
|
|
/// <summary>
|
|
/// 检查GCP目标转角是否为有限值。
|
|
/// </summary>
|
|
private static void EnsureFinite(
|
|
double value,
|
|
string parameterName)
|
|
{
|
|
if (double.IsNaN(value) ||
|
|
double.IsInfinity(value))
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
parameterName,
|
|
"GCP目标转角必须是有限值。");
|
|
}
|
|
}
|
|
}
|
|
}
|