321 lines
12 KiB
C#
321 lines
12 KiB
C#
|
||
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using ClumsyCore.Interfaces;
|
||
using ClumsyCore.Pilot;
|
||
using CommonUsage.Chassis;
|
||
using MDCSToolBox.Commons.Controllers;
|
||
using MyParking.Shared;
|
||
using MultiWheelC.StateEstimation;
|
||
|
||
namespace MultiWheelC
|
||
{
|
||
public class MultiWheelRotateInPlace : MovementDefinition
|
||
{
|
||
/// <summary>
|
||
/// 旋转目标角度
|
||
/// </summary>
|
||
public float AngleTarget;
|
||
|
||
// 留作标定或单元测试时显式替换；为空时使用经过校验的Detour状态源。
|
||
public Func<float> ThetaReader;
|
||
|
||
public IVehicleStateProvider StateProvider =
|
||
new DetourVehicleStateProvider();
|
||
|
||
public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||
|
||
public Func<PIDParams> PidparamsRead = () => new PIDParams() { };
|
||
|
||
public PIDController thPid;
|
||
|
||
// 将本周期PID角速度输出提供给实验记录器，单位deg/s。
|
||
public Action<float> CommandAngularSpeedObserver;
|
||
|
||
// 自转前舵轮实际角度允许误差，单位deg。
|
||
public float WheelAlignmentToleranceDegrees = 2f;
|
||
|
||
// 自转舵轮连续保持到位的时间，单位s。
|
||
public float WheelAlignmentStableSeconds = 0.3f;
|
||
|
||
// 自转舵轮准备超时时间，单位s。
|
||
public float WheelAlignmentTimeoutSeconds = 10f;
|
||
|
||
// 航向尚未到位时允许下发的最小有效角速度，单位deg/s。
|
||
public float MinimumAngularSpeedDegreesPerSecond = 1f;
|
||
|
||
// 舵轮到位后执行航向闭环允许的最长时间，单位s。
|
||
public float RotationTimeoutSeconds = 15f;
|
||
|
||
// 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。
|
||
public override IEnumerable<bool> Get()
|
||
{
|
||
if (Chassis == null)
|
||
throw new InvalidOperationException(
|
||
"当前底盘不是MultiWheelChassis，无法执行原地自转。");
|
||
|
||
ValidateParameters();
|
||
|
||
var adapter = new MultiWheelChassisAdapter(
|
||
Chassis,
|
||
PilotDefinition.Self.CarNum);
|
||
adapter.ResetToBodyFrame();
|
||
|
||
try
|
||
{
|
||
var alignmentStarted = DateTime.Now;
|
||
DateTime? alignedSince = null;
|
||
while (true)
|
||
{
|
||
if (!adapter.PrepareSpin(
|
||
alignmentToleranceDegrees:
|
||
WheelAlignmentToleranceDegrees))
|
||
throw new InvalidOperationException(
|
||
"无法生成原地自转舵轮目标：" +
|
||
adapter.LastFailureReason);
|
||
|
||
if (adapter.AreSpinWheelsAligned)
|
||
{
|
||
if (alignedSince == null)
|
||
alignedSince = DateTime.Now;
|
||
|
||
if ((DateTime.Now - alignedSince.Value)
|
||
.TotalSeconds >=
|
||
WheelAlignmentStableSeconds)
|
||
break;
|
||
}
|
||
else
|
||
{
|
||
alignedSince = null;
|
||
}
|
||
|
||
if ((DateTime.Now - alignmentStarted)
|
||
.TotalSeconds >
|
||
WheelAlignmentTimeoutSeconds)
|
||
throw new TimeoutException(
|
||
"原地自转舵轮在限定时间内未稳定到位。");
|
||
|
||
yield return true;
|
||
}
|
||
|
||
var alignmentToleranceRadians =
|
||
AngleMath.DegreesToRadians(
|
||
WheelAlignmentToleranceDegrees);
|
||
if (!adapter.AdoptPreparedSpinForXYTh(
|
||
alignmentToleranceRadians))
|
||
{
|
||
throw new InvalidOperationException(
|
||
"无法将已到位的自转舵角交接给XYTh：" +
|
||
adapter.LastFailureReason);
|
||
}
|
||
|
||
var targetAngle =
|
||
(float)AngleMath.NormalizeDegrees(AngleTarget);
|
||
var p = PidparamsRead();
|
||
var currentAngle = ReadCurrentAngleDegrees();
|
||
var cachedCurrentAngle = currentAngle;
|
||
thPid = new PIDController(
|
||
() => cachedCurrentAngle,
|
||
p.Kp);
|
||
thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
|
||
p.OutputUpperThreshold, p.SpeedAccPerSec);
|
||
var lastCommandTime = DateTime.Now;
|
||
var rotationStarted = DateTime.Now;
|
||
|
||
while (true)
|
||
{
|
||
if ((DateTime.Now - rotationStarted)
|
||
.TotalSeconds >
|
||
RotationTimeoutSeconds)
|
||
{
|
||
throw new TimeoutException(
|
||
$"原地自转超过{RotationTimeoutSeconds:F1}s仍未到位。");
|
||
}
|
||
|
||
currentAngle = ReadCurrentAngleDegrees();
|
||
cachedCurrentAngle = currentAngle;
|
||
var s = thPid.GetResponse(targetAngle, true);
|
||
var angleErrorDegrees =
|
||
(float)AngleMath
|
||
.ShortestDifferenceDegrees(
|
||
targetAngle,
|
||
currentAngle);
|
||
|
||
// PID进入到位死区后等待其0.3s稳定确认；等待期间
|
||
// 只清零驱动速度，不清除已经准备好的自转舵角状态。
|
||
if (Math.Abs(angleErrorDegrees) <=
|
||
p.DeadZone)
|
||
{
|
||
CommandAngularSpeedObserver?.Invoke(0f);
|
||
adapter
|
||
.StopXYThDrivePreserveSteeringState();
|
||
|
||
if (thPid.IsArrived())
|
||
break;
|
||
|
||
yield return true;
|
||
continue;
|
||
}
|
||
|
||
// PID输出低于底盘有效轮速范围时提高到最小可执行值，
|
||
// 避免接近目标时反复出现微小命令但车辆实际不动。
|
||
if (Math.Abs(s) > 1e-6f &&
|
||
Math.Abs(s) <
|
||
MinimumAngularSpeedDegreesPerSecond)
|
||
{
|
||
s = Math.Sign(angleErrorDegrees) *
|
||
MinimumAngularSpeedDegreesPerSecond;
|
||
}
|
||
|
||
// PID加速限制在首周期可能暂时输出零；此时保留
|
||
// 已交接的自转状态，等待下一周期产生有效角速度。
|
||
if (Math.Abs(s) <= 1e-6f)
|
||
{
|
||
CommandAngularSpeedObserver?.Invoke(0f);
|
||
adapter
|
||
.StopXYThDrivePreserveSteeringState();
|
||
yield return true;
|
||
continue;
|
||
}
|
||
|
||
CommandAngularSpeedObserver?.Invoke(s);
|
||
var now = DateTime.Now;
|
||
var interval = now - lastCommandTime;
|
||
lastCommandTime = now;
|
||
|
||
// PID输出s为deg/s，Shared命令统一使用rad/s。
|
||
// adapter.Send最终调用普通安全版SendXYThSpeed。
|
||
var omegaRadiansPerSecond =
|
||
(float)AngleMath.DegreesToRadians(s);
|
||
if (!adapter.Send(
|
||
new ChassisCommand(
|
||
PilotDefinition.Self.CarNum,
|
||
new Twist2D(
|
||
0.0,
|
||
0.0,
|
||
omegaRadiansPerSecond)),
|
||
interval))
|
||
{
|
||
throw new InvalidOperationException(
|
||
"安全XYTh原地旋转底盘解算失败：" +
|
||
adapter.LastFailureReason);
|
||
}
|
||
yield return true;
|
||
}
|
||
|
||
Console.WriteLine($"final rotate to {targetAngle}");
|
||
}
|
||
finally
|
||
{
|
||
CommandAngularSpeedObserver?.Invoke(0f);
|
||
adapter.StopImmediately();
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查原地自转的舵轮准备、最小速度和超时参数是否可执行。
|
||
/// </summary>
|
||
private void ValidateParameters()
|
||
{
|
||
EnsureFinitePositive(
|
||
WheelAlignmentToleranceDegrees,
|
||
nameof(WheelAlignmentToleranceDegrees),
|
||
allowZero: true);
|
||
EnsureFinitePositive(
|
||
WheelAlignmentStableSeconds,
|
||
nameof(WheelAlignmentStableSeconds),
|
||
allowZero: true);
|
||
EnsureFinitePositive(
|
||
WheelAlignmentTimeoutSeconds,
|
||
nameof(WheelAlignmentTimeoutSeconds));
|
||
EnsureFinitePositive(
|
||
MinimumAngularSpeedDegreesPerSecond,
|
||
nameof(MinimumAngularSpeedDegreesPerSecond));
|
||
EnsureFinitePositive(
|
||
RotationTimeoutSeconds,
|
||
nameof(RotationTimeoutSeconds));
|
||
|
||
var pidParameters = PidparamsRead();
|
||
if (pidParameters == null)
|
||
{
|
||
throw new InvalidOperationException(
|
||
"原地自转PID参数读取结果为空。");
|
||
}
|
||
|
||
EnsureFinitePositive(
|
||
pidParameters.DeadZone,
|
||
"PidparamsRead.DeadZone");
|
||
EnsureFinitePositive(
|
||
pidParameters.OutputUpperThreshold,
|
||
"PidparamsRead.OutputUpperThreshold");
|
||
EnsureFinitePositive(
|
||
pidParameters.SpeedAccPerSec,
|
||
"PidparamsRead.SpeedAccPerSec");
|
||
EnsureFinitePositive(
|
||
pidParameters.Kp,
|
||
"PidparamsRead.Kp");
|
||
|
||
if (MinimumAngularSpeedDegreesPerSecond >
|
||
pidParameters.OutputUpperThreshold)
|
||
{
|
||
throw new InvalidOperationException(
|
||
"原地自转最小有效角速度不能大于最大角速度。");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 读取经过状态源校验的世界航向，显式设置ThetaReader时优先使用替代读数。
|
||
/// </summary>
|
||
private float ReadCurrentAngleDegrees()
|
||
{
|
||
if (ThetaReader != null)
|
||
{
|
||
var angleDegrees = ThetaReader();
|
||
if (float.IsNaN(angleDegrees) ||
|
||
float.IsInfinity(angleDegrees))
|
||
{
|
||
throw new InvalidOperationException(
|
||
"自定义航向读取结果不是有效角度。");
|
||
}
|
||
|
||
return (float)AngleMath.NormalizeDegrees(
|
||
angleDegrees);
|
||
}
|
||
|
||
if (StateProvider == null ||
|
||
!StateProvider.TryGetState(out var state))
|
||
{
|
||
throw new InvalidOperationException(
|
||
"无法从Detour状态源读取有效车辆航向。" +
|
||
(StateProvider is DetourVehicleStateProvider provider
|
||
? provider.LastFailureReason
|
||
: ""));
|
||
}
|
||
|
||
return (float)AngleMath.RadiansToDegrees(
|
||
state.PoseInWorld.YawRadians);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查原地自转参数是否为正有限值，部分时间和容差参数允许为零。
|
||
/// </summary>
|
||
private static void EnsureFinitePositive(
|
||
float value,
|
||
string parameterName,
|
||
bool allowZero = false)
|
||
{
|
||
if (float.IsNaN(value) ||
|
||
float.IsInfinity(value) ||
|
||
(allowZero
|
||
? value < 0f
|
||
: value <= 0f))
|
||
{
|
||
throw new ArgumentOutOfRangeException(
|
||
parameterName,
|
||
"原地自转参数必须是有效的正数。");
|
||
}
|
||
}
|
||
}
|
||
}
|