完善原地自转控制逻辑并加入纵向速度死区与实验绘图改进
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Cursor
@@ -1,6 +1,7 @@
|
||||
|
||||
|
||||
using System;
|
||||
using System.Globalization;
|
||||
using System.Numerics;
|
||||
using System.Threading;
|
||||
using ClumsyCore;
|
||||
@@ -17,7 +18,6 @@ namespace MultiWheelC
|
||||
public abstract class InPlaceRotateTestBase : MovementTest
|
||||
{
|
||||
public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。
|
||||
public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。
|
||||
public int TrialNumber = 1; // 重复实验编号。
|
||||
|
||||
private DriveTask _task;
|
||||
@@ -37,11 +37,18 @@ namespace MultiWheelC
|
||||
// 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。
|
||||
public override void Test()
|
||||
{
|
||||
var config = PilotDefinition.Conf;
|
||||
|
||||
if (float.IsNaN(RelativeAngleDegrees) ||
|
||||
float.IsInfinity(RelativeAngleDegrees) ||
|
||||
float.IsNaN(MaxAngularSpeedDegreesPerSecond) ||
|
||||
float.IsInfinity(MaxAngularSpeedDegreesPerSecond) ||
|
||||
MaxAngularSpeedDegreesPerSecond <= 0f)
|
||||
float.IsNaN(config.InPlaceRotateMaxSpeed) ||
|
||||
float.IsInfinity(config.InPlaceRotateMaxSpeed) ||
|
||||
config.InPlaceRotateMaxSpeed <= 0f ||
|
||||
float.IsNaN(config.InPlaceRotateMinimumSpeed) ||
|
||||
float.IsInfinity(config.InPlaceRotateMinimumSpeed) ||
|
||||
config.InPlaceRotateMinimumSpeed <= 0f ||
|
||||
config.InPlaceRotateMinimumSpeed >
|
||||
config.InPlaceRotateMaxSpeed)
|
||||
{
|
||||
Console.WriteLine("原地旋转测试参数无效。");
|
||||
return;
|
||||
@@ -66,8 +73,25 @@ namespace MultiWheelC
|
||||
(float)AngleMath.NormalizeDegrees(
|
||||
location.th + RelativeAngleDegrees);
|
||||
|
||||
Console.WriteLine(
|
||||
"原地自转实际参数：" +
|
||||
$"Kp={config.InPlaceRotateKp:F3}，" +
|
||||
$"Ki={config.InPlaceRotateKi:F3}，" +
|
||||
$"Kd={config.InPlaceRotateKd:F3}，" +
|
||||
$"到位误差={config.InPlaceRotateArriveDeg:F2}°，" +
|
||||
$"最小角速度={config.InPlaceRotateMinimumSpeed:F2}°/s，" +
|
||||
$"最大角速度={config.InPlaceRotateMaxSpeed:F2}°/s，" +
|
||||
$"角加速度={config.InPlaceRotateAcc:F2}°/s²，" +
|
||||
$"舵轮到位误差={config.InPlaceRotateWheelAlignDeg:F2}°，" +
|
||||
$"旋转超时={config.InPlaceRotateTimeoutSec:F1}s；" +
|
||||
$"起点航向={location.th:F2}°，" +
|
||||
$"目标航向={targetWorldAngle:F2}°。");
|
||||
Console.WriteLine(
|
||||
"原地自转CSV保存目录：" +
|
||||
TrackingExperimentRecorder.DefaultOutputDirectory);
|
||||
|
||||
_recorder = new TrackingExperimentRecorder(
|
||||
controllerName: "InPlaceRotatePID",
|
||||
controllerName: "InPlaceRotateFilteredPID",
|
||||
trajectoryName: _trajectoryName,
|
||||
trialNumber: TrialNumber,
|
||||
referenceStart: rotationCenter,
|
||||
@@ -75,7 +99,7 @@ namespace MultiWheelC
|
||||
referenceSpeed: 0f,
|
||||
referenceAngularSpeed:
|
||||
(float)AngleMath.DegreesToRadians(
|
||||
MaxAngularSpeedDegreesPerSecond));
|
||||
config.InPlaceRotateMaxSpeed));
|
||||
_recorder.Start();
|
||||
|
||||
try
|
||||
@@ -88,21 +112,26 @@ namespace MultiWheelC
|
||||
PidparamsRead = () => new PIDParams
|
||||
{
|
||||
Kp =
|
||||
PilotDefinition.Conf.InPlaceRotateKp,
|
||||
config.InPlaceRotateKp,
|
||||
Ki =
|
||||
PilotDefinition.Conf.InPlaceRotateKi,
|
||||
config.InPlaceRotateKi,
|
||||
Kd =
|
||||
PilotDefinition.Conf.InPlaceRotateKd,
|
||||
config.InPlaceRotateKd,
|
||||
DeadZone =
|
||||
PilotDefinition.Conf
|
||||
.InPlaceRotateArriveDeg,
|
||||
config.InPlaceRotateArriveDeg,
|
||||
SpeedAccPerSec =
|
||||
PilotDefinition.Conf.InPlaceRotateAcc,
|
||||
config.InPlaceRotateAcc,
|
||||
OutputUpperThreshold =
|
||||
MaxAngularSpeedDegreesPerSecond,
|
||||
config.InPlaceRotateMaxSpeed,
|
||||
MaxI =
|
||||
PilotDefinition.Conf.InPlaceRotateMaxI
|
||||
config.InPlaceRotateMaxI
|
||||
},
|
||||
MinimumAngularSpeedDegreesPerSecond =
|
||||
config.InPlaceRotateMinimumSpeed,
|
||||
WheelAlignmentToleranceDegrees =
|
||||
config.InPlaceRotateWheelAlignDeg,
|
||||
RotationTimeoutSeconds =
|
||||
config.InPlaceRotateTimeoutSec,
|
||||
CommandAngularSpeedObserver =
|
||||
commandAngularSpeed =>
|
||||
_recorder?.UpdateCommand(
|
||||
@@ -136,23 +165,58 @@ namespace MultiWheelC
|
||||
|
||||
}
|
||||
|
||||
[MovementTest(name = "SendXYThSpeed：原地自转90°")]
|
||||
public sealed class TestRotate90 :
|
||||
[MovementTest(name = "SendXYThSpeed：输入角度原地自转")]
|
||||
public sealed class TestRotateAngle :
|
||||
InPlaceRotateTestBase
|
||||
{
|
||||
public TestRotate90()
|
||||
: base(90f, "Rotate90")
|
||||
public TestRotateAngle()
|
||||
: base(0f, "RotateCustomAngle")
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取相对旋转角度并按正值逆时针、负值顺时针执行原地自转。
|
||||
/// </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))
|
||||
{
|
||||
Console.WriteLine("旋转角度输入无效，测试已经取消。");
|
||||
return;
|
||||
}
|
||||
|
||||
if (Math.Abs(relativeAngleDegrees) < 1e-3f)
|
||||
{
|
||||
Console.WriteLine("旋转角度不能为0，测试已经取消。");
|
||||
return;
|
||||
}
|
||||
|
||||
// 当前控制器按照圆周最短角旋转；精确±180°的方向存在二义性。
|
||||
if (Math.Abs(relativeAngleDegrees) >= 180f)
|
||||
{
|
||||
Console.WriteLine(
|
||||
"输入角度必须满足-180° < angle < 180°；" +
|
||||
"当前最短角控制不支持指定精确±180°的旋转方向。");
|
||||
return;
|
||||
}
|
||||
|
||||
RelativeAngleDegrees = relativeAngleDegrees;
|
||||
base.Test();
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "SendXYThSpeed：原地自转180°")]
|
||||
public sealed class TestRotate180 :
|
||||
InPlaceRotateTestBase
|
||||
{
|
||||
public TestRotate180()
|
||||
: base(180f, "Rotate180")
|
||||
{
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user