74 lines
2.4 KiB
C#
74 lines
2.4 KiB
C#
using System;
|
|
using MultiWheelC.Control.Abstractions;
|
|
using MyParking.Shared;
|
|
// 限制目标角度的最大绝对值，例如不能超过60°。
|
|
namespace MultiWheelC.Control.Allocation
|
|
{
|
|
/// <summary>
|
|
/// 独立限制前后GCP目标转角并与纵向速度组合成底盘运动命令。
|
|
/// </summary>
|
|
public sealed class GcpCommandAllocator
|
|
{
|
|
/// <summary>
|
|
/// 创建使用指定前后GCP最大转角的命令分配器。
|
|
/// </summary>
|
|
public GcpCommandAllocator(double maximumGcpAngleRadians)
|
|
{
|
|
NumericGuard.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)
|
|
{
|
|
NumericGuard.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));
|
|
}
|
|
|
|
}
|
|
}
|