增加车队轨迹控制核心与轮组自转模式
This commit is contained in:
@@ -20,6 +20,8 @@ namespace MultiWheelC
|
||||
{
|
||||
public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。
|
||||
public int TrialNumber = 1; // 重复实验编号。
|
||||
public InPlaceRotationFeedbackMode FeedbackMode =
|
||||
InPlaceRotationFeedbackMode.DetourAbsoluteHeading;
|
||||
|
||||
private DriveTask _task;
|
||||
private TrackingExperimentRecorder _recorder;
|
||||
@@ -93,6 +95,12 @@ namespace MultiWheelC
|
||||
var targetWorldAngle =
|
||||
(float)AngleMath.NormalizeDegrees(
|
||||
location.th + RelativeAngleDegrees);
|
||||
var movementAngleTarget =
|
||||
FeedbackMode ==
|
||||
InPlaceRotationFeedbackMode
|
||||
.RelativeWheelOdometry
|
||||
? RelativeAngleDegrees
|
||||
: targetWorldAngle;
|
||||
|
||||
Console.WriteLine(
|
||||
"原地自转实际参数：" +
|
||||
@@ -106,13 +114,19 @@ namespace MultiWheelC
|
||||
$"舵轮到位误差={config.InPlaceRotateWheelAlignDeg:F2}°，" +
|
||||
$"旋转超时={config.InPlaceRotateTimeoutSec:F1}s；" +
|
||||
$"起点航向={location.th:F2}°，" +
|
||||
$"目标航向={targetWorldAngle:F2}°。");
|
||||
$"目标航向={targetWorldAngle:F2}°，" +
|
||||
$"反馈模式={FeedbackMode}。");
|
||||
Console.WriteLine(
|
||||
"原地自转CSV保存目录：" +
|
||||
TrackingExperimentRecorder.DefaultOutputDirectory);
|
||||
|
||||
_recorder = new TrackingExperimentRecorder(
|
||||
controllerName: "InPlaceRotateFilteredPID",
|
||||
controllerName:
|
||||
FeedbackMode ==
|
||||
InPlaceRotationFeedbackMode
|
||||
.RelativeWheelOdometry
|
||||
? "InPlaceRotateWheelOdometry"
|
||||
: "InPlaceRotateFilteredPID",
|
||||
trajectoryName: _trajectoryName,
|
||||
trialNumber: TrialNumber,
|
||||
referenceStart: rotationCenter,
|
||||
@@ -130,8 +144,8 @@ namespace MultiWheelC
|
||||
_task = new DriveTask(
|
||||
new MultiWheelRotateInPlace
|
||||
{
|
||||
// MultiWheelRotateInPlace接收世界坐标系绝对航向。
|
||||
AngleTarget = targetWorldAngle,
|
||||
AngleTarget = movementAngleTarget,
|
||||
FeedbackMode = FeedbackMode,
|
||||
Chassis = chassis,
|
||||
StateProvider = stateProvider,
|
||||
CommandAngularSpeedObserver =
|
||||
@@ -165,6 +179,46 @@ namespace MultiWheelC
|
||||
_recorder?.StopAndSave();
|
||||
}
|
||||
|
||||
// 读取并校验测试使用的有符号相对旋转角度。
|
||||
protected static bool TryReadRelativeAngleDegrees(
|
||||
out float relativeAngleDegrees)
|
||||
{
|
||||
var input = UI.GetInput(
|
||||
"输入相对旋转角度（deg，正数逆时针，负数顺时针，范围-180到180之间）：");
|
||||
|
||||
if ((!float.TryParse(
|
||||
input,
|
||||
NumberStyles.Float,
|
||||
CultureInfo.CurrentCulture,
|
||||
out relativeAngleDegrees) &&
|
||||
!float.TryParse(
|
||||
input,
|
||||
NumberStyles.Float,
|
||||
CultureInfo.InvariantCulture,
|
||||
out relativeAngleDegrees)) ||
|
||||
float.IsNaN(relativeAngleDegrees) ||
|
||||
float.IsInfinity(relativeAngleDegrees))
|
||||
{
|
||||
Console.WriteLine("旋转角度输入无效，测试已经取消。");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (Math.Abs(relativeAngleDegrees) < 1e-3f)
|
||||
{
|
||||
Console.WriteLine("旋转角度不能为0，测试已经取消。");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (Math.Abs(relativeAngleDegrees) >= 180f)
|
||||
{
|
||||
Console.WriteLine(
|
||||
"输入角度必须满足-180° < angle < 180°。");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
[MovementTest(name = "SendXYThSpeed：输入角度原地自转")]
|
||||
@@ -181,58 +235,37 @@ namespace MultiWheelC
|
||||
/// </summary>
|
||||
public override void Test()
|
||||
{
|
||||
var input = UI.GetInput(
|
||||
"输入相对旋转角度（deg，正数逆时针，负数顺时针，范围-180到180之间）：");
|
||||
|
||||
if ((!float.TryParse(
|
||||
input,
|
||||
NumberStyles.Float,
|
||||
CultureInfo.CurrentCulture,
|
||||
out var relativeAngleDegrees) &&
|
||||
!float.TryParse(
|
||||
input,
|
||||
NumberStyles.Float,
|
||||
CultureInfo.InvariantCulture,
|
||||
out relativeAngleDegrees)) ||
|
||||
float.IsNaN(relativeAngleDegrees) ||
|
||||
float.IsInfinity(relativeAngleDegrees))
|
||||
if (!TryReadRelativeAngleDegrees(
|
||||
out var relativeAngleDegrees))
|
||||
{
|
||||
Console.WriteLine("旋转角度输入无效，测试已经取消。");
|
||||
return;
|
||||
}
|
||||
|
||||
var chassis =
|
||||
PilotDefinition.Chassis as MultiWheelChassis;
|
||||
if (chassis == null)
|
||||
{
|
||||
Console.WriteLine(
|
||||
"当前底盘不是MultiWheelChassis，无法执行原地旋转测试。");
|
||||
return;
|
||||
}
|
||||
RelativeAngleDegrees = relativeAngleDegrees;
|
||||
base.Test();
|
||||
}
|
||||
}
|
||||
|
||||
var stateProvider =
|
||||
ParkingVehicleStateProviderFactory.Create(
|
||||
chassis);
|
||||
if (!stateProvider.TryGetState(out _))
|
||||
{
|
||||
Console.WriteLine(
|
||||
"无法读取原地旋转起点状态：" +
|
||||
stateProvider.LastFailureReason);
|
||||
return;
|
||||
}
|
||||
[MovementTest(name = "轮组里程计：输入角度原地相对自转")]
|
||||
public sealed class TestWheelOdometryRotateAngle :
|
||||
InPlaceRotateTestBase
|
||||
{
|
||||
public TestWheelOdometryRotateAngle()
|
||||
: base(0f, "RotateWheelOdometryCustomAngle")
|
||||
{
|
||||
FeedbackMode =
|
||||
InPlaceRotationFeedbackMode
|
||||
.RelativeWheelOdometry;
|
||||
}
|
||||
|
||||
if (Math.Abs(relativeAngleDegrees) < 1e-3f)
|
||||
/// <summary>
|
||||
/// 读取相对角度并仅用滤波后的轮组角速度积分完成自转。
|
||||
/// </summary>
|
||||
public override void Test()
|
||||
{
|
||||
if (!TryReadRelativeAngleDegrees(
|
||||
out var relativeAngleDegrees))
|
||||
{
|
||||
Console.WriteLine("旋转角度不能为0，测试已经取消。");
|
||||
return;
|
||||
}
|
||||
|
||||
// 当前控制器按照圆周最短角旋转；精确±180°的方向存在二义性。
|
||||
if (Math.Abs(relativeAngleDegrees) >= 180f)
|
||||
{
|
||||
Console.WriteLine(
|
||||
"输入角度必须满足-180° < angle < 180°；" +
|
||||
"当前最短角控制不支持指定精确±180°的旋转方向。");
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user