105 lines
3.2 KiB
C#
105 lines
3.2 KiB
C#
using System;
|
|
using MultiWheelC.Control.Abstractions;
|
|
|
|
namespace MultiWheelC.Control.Allocation
|
|
{
|
|
/// <summary>
|
|
/// 独立限制前后GCP目标转角并与纵向速度组合成底盘运动命令。
|
|
/// </summary>
|
|
public sealed class GcpCommandAllocator
|
|
{
|
|
/// <summary>
|
|
/// 创建使用指定前后GCP最大转角的命令分配器。
|
|
/// </summary>
|
|
public GcpCommandAllocator(double maximumGcpAngleRadians)
|
|
{
|
|
EnsureFinitePositive(
|
|
maximumGcpAngleRadians,
|
|
nameof(maximumGcpAngleRadians));
|
|
|
|
if (maximumGcpAngleRadians >= Math.PI / 2.0)
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
nameof(maximumGcpAngleRadians),
|
|
"最大GCP转角必须小于π/2。");
|
|
}
|
|
|
|
MaximumGcpAngleRadians = maximumGcpAngleRadians;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取前后GCP允许的最大转角绝对值，单位为rad。
|
|
/// </summary>
|
|
public double MaximumGcpAngleRadians { get; }
|
|
|
|
/// <summary>
|
|
/// 将纵向速度和前后GCP转角组合为底盘运动命令。
|
|
/// </summary>
|
|
public GcpMotionCommand Allocate(
|
|
double speedMetersPerSecond,
|
|
LateralControlCommand lateralCommand)
|
|
{
|
|
EnsureFinite(
|
|
speedMetersPerSecond,
|
|
nameof(speedMetersPerSecond));
|
|
|
|
var frontAngleRadians = ClampSymmetric(
|
|
lateralCommand.FrontGcpAngleRadians,
|
|
MaximumGcpAngleRadians);
|
|
var rearAngleRadians = ClampSymmetric(
|
|
lateralCommand.RearGcpAngleRadians,
|
|
MaximumGcpAngleRadians);
|
|
|
|
return new GcpMotionCommand(
|
|
speedMetersPerSecond,
|
|
frontAngleRadians,
|
|
rearAngleRadians);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 将数值按正负对称方式限制在指定绝对值内。
|
|
/// </summary>
|
|
private static double ClampSymmetric(
|
|
double value,
|
|
double maximumAbsoluteValue)
|
|
{
|
|
return Math.Max(
|
|
-maximumAbsoluteValue,
|
|
Math.Min(maximumAbsoluteValue, value));
|
|
}
|
|
|
|
/// <summary>
|
|
/// 检查参数是否为正有限值。
|
|
/// </summary>
|
|
private static void EnsureFinitePositive(
|
|
double value,
|
|
string parameterName)
|
|
{
|
|
EnsureFinite(value, parameterName);
|
|
|
|
if (value <= 0.0)
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
parameterName,
|
|
"GCP分配参数必须是正有限值。");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 检查参数或命令是否为有限值。
|
|
/// </summary>
|
|
private static void EnsureFinite(
|
|
double value,
|
|
string parameterName)
|
|
{
|
|
if (double.IsNaN(value) ||
|
|
double.IsInfinity(value))
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
parameterName,
|
|
"GCP分配参数和命令必须是有限值。");
|
|
}
|
|
}
|
|
}
|
|
}
|