First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

完善轨迹跟踪测试并添加实验数据记录与绘图分析工具

Browse Source 
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
yuxiang.shen
2026-07-28 18:14:33 +08:00
co-authored by Cursor
parent e6b99c45b3
commit 8e8af847db
90 changed files with 2852 additions and 123 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/MovementTests.cs
+589 -95
View File
@@ -2,125 +2,619 @@ using ClumsyCore;
using ClumsyCore.DTools;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using MDCSToolBox.Commons.Controllers;
using MDCSToolBox.Clumsy.Tracks;
using MyParking.Shared;
using System;
using System.Numerics;
using System.Threading;
namespace MultiWheelC
{
public abstract class DstTrackerTestBase : MovementTest
internal static class MovementTestPreparation
{
public bool UseInteractivePick = true;
public float srcX;
public float srcY;
public float dstX;
public float dstY;
public float carDirectionBias;
private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
private DriveTask _dt;
protected DstTrackerTestBase(float defaultCarDirectionBias)
// 在测试正式开始前，将四个舵轮稳定回正到车体前向。
public static bool AlignWheelsForward(
ref DriveTask activeTask)
{
carDirectionBias = defaultCarDirectionBias;
}
var preparation = new PrepareWheelsForward();
var task = new DriveTask(preparation.Get());
activeTask = task;
public override void TestStop()
{
_dt?.Stop();
_painter?.Clear();
}
public override void Test()
{
Vector2 p1;
Vector2 p2;
if (UseInteractivePick)
try
{
p1 = UI.GetPoint("point1");
p2 = UI.GetPoint("point2");
task.Wait();
return preparation.Completed;
}
else
{
p1 = new Vector2(srcX, srcY);
p2 = new Vector2(dstX, dstY);
}
_painter.Clear();
_dt = new DriveTask(new DstTracker
{
Src = p1,
Dst = p2,
CarDirectionBias = carDirectionBias,
}.Get());
_dt.Wait();
}
}
[MovementTest(name = "测试终点跟踪动作-前进")]
public sealed class DstTrackerForward : DstTrackerTestBase
{
public DstTrackerForward() : base(0f) { }
}
[MovementTest(name = "测试终点跟踪动作-后退")]
public sealed class DstTrackerBackward : DstTrackerTestBase
{
public DstTrackerBackward() : base(180f) { }
}
[MovementTest(name = "底盘旋转测试")]
public class RotateToAngleTest : MovementTest
{
private DriveTask _dt;
// 停止当前正在执行的底盘原地旋转任务。
public override void TestStop()
{
_dt?.Stop();
}
// 交互输入目标角度后执行底盘原地旋转测试。
public override void Test()
{
var input = UI.GetInput("输入旋转角度：");
if (!float.TryParse(input, out var angleTarget))
catch (Exception ex)
{
Console.WriteLine(
$"旋转测试输入无效：{input}");
$"测试前舵轮回正失败：{ex.Message}");
return false;
}
finally
{
task.Stop();
if (ReferenceEquals(activeTask, task))
activeTask = null;
}
}
// 只读取实际舵角，检查四个舵轮是否已与车头方向一致。
public static bool AreWheelsForward(
float toleranceDegrees = 2f)
{
var chassis =
PilotDefinition.Chassis as MultiWheelChassis;
if (chassis == null)
{
Console.WriteLine(
"当前底盘不是MultiWheelChassis，无法检查舵轮方向。");
return false;
}
try
{
var adapter = new MultiWheelChassisAdapter(
chassis,
PilotDefinition.Self.CarNum);
var toleranceRadians =
toleranceDegrees * Math.PI / 180.0;
if (adapter.AreParallelWheelsAligned(
0.0,
toleranceRadians))
{
return true;
}
Console.WriteLine(
"四个舵轮尚未与车头方向一致，请先执行“准备：四个舵轮与车头方向一致”。");
return false;
}
catch (Exception ex)
{
Console.WriteLine(
$"检查舵轮方向失败：{ex.Message}");
return false;
}
}
}
[MovementTest(name = "准备：四个舵轮与车头方向一致")]
public class AlignWheelsForwardTest : MovementTest
{
private DriveTask _task;
// 单独将四个舵轮转到车体前向0°并等待实际反馈稳定到位。
public override void Test()
{
MovementTestPreparation.AlignWheelsForward(
ref _task);
}
// 停止正在执行的舵轮回正任务并清零底盘运动命令。
public override void TestStop()
{
_task?.Stop();
_task = null;
}
}
[MovementTest(name = "测试连续前进4m")]
public class TestForward4m : MovementTest
{
public float DistanceMillimeters = 4000f; // 测试距离，单位mm。
public float CruiseSpeed = 0.3f; // 巡航速度上限，单位m/s。
public int TrialNumber = 1; // 重复实验编号。
private DriveTask _task;
private TrackingExperimentRecorder _recorder;
// 从当前Detour位置沿车头方向生成4m连续直线并记录测试数据。
public override void Test()
{
if (!MovementTestPreparation.AreWheelsForward())
{
return;
}
// 防止重复启动测试时，上一项旋转任务仍在运行。
_dt?.Stop();
var task = new DriveTask(
new MultiWheelRotateInPlace
{
AngleTarget = angleTarget,
PidparamsRead = () => new PIDParams
{
Kp = PilotDefinition.Conf.InPlaceRotateKp,
Ki = PilotDefinition.Conf.InPlaceRotateKi,
Kd = PilotDefinition.Conf.InPlaceRotateKd,
DeadZone = PilotDefinition.Conf.InPlaceRotateArriveDeg,
SpeedAccPerSec = PilotDefinition.Conf.InPlaceRotateAcc,
OutputUpperThreshold = PilotDefinition.Conf.InPlaceRotateMaxSpeed,
MaxI = PilotDefinition.Conf.InPlaceRotateMaxI,
}
var location = DetourInterface.getCartLocation();
if (double.IsNaN(location.x) ||
double.IsInfinity(location.x) ||
double.IsNaN(location.y) ||
double.IsInfinity(location.y) ||
double.IsNaN(location.th) ||
double.IsInfinity(location.th))
{
Console.WriteLine(
"Detour当前位姿无效，取消连续前进4m测试。");
return;
}
var source = new Vector2((float)location.x, (float)location.y);
// Detour航向单位是度，三角函数需要弧度。
var headingRadians = location.th * Math.PI / 180.0;
var destination = new Vector2(
source.X + DistanceMillimeters * (float)Math.Cos(headingRadians),
source.Y + DistanceMillimeters * (float)Math.Sin(headingRadians));
_recorder =
new TrackingExperimentRecorder(
controllerName: "Stanley",
trajectoryName: "Straight4m",
trialNumber: TrialNumber,
referenceStart: source,
referenceEnd: destination,
referenceSpeed: CruiseSpeed);
_recorder.Start();
try
{
_task = new DriveTask(
new DstTracker
{
Src = source,
Dst = destination,
CarDirectionBias = 0f,
MaxSpeed = CruiseSpeed
}.Get());
_dt = task;
_task.Wait();
// 保留少量停车后数据，便于观察速度是否回到零。
Thread.Sleep(300);
}
finally
{
_task?.Stop();
_recorder?.UpdateCommand(0f, 0f);
_recorder?.StopAndSave();
_task = null;
_recorder = null;
}
}
public override void TestStop()
{
_task?.Stop();
_recorder?.UpdateCommand(0f, 0f);
_recorder?.StopAndSave();
}
}
[MovementTest(name = "测试原地自转90°")]
public class TestRotate90 : MovementTest
{
public float RelativeAngleDegrees = 90f; // 相对当前航向的旋转角度，逆时针为正。
public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。
public int TrialNumber = 1; // 重复实验编号。
private DriveTask _task;
private TrackingExperimentRecorder _recorder;
// 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。
public override void Test()
{
if (float.IsNaN(RelativeAngleDegrees) ||
float.IsInfinity(RelativeAngleDegrees) ||
float.IsNaN(MaxAngularSpeedDegreesPerSecond) ||
float.IsInfinity(MaxAngularSpeedDegreesPerSecond) ||
MaxAngularSpeedDegreesPerSecond <= 0f)
{
Console.WriteLine("原地旋转测试参数无效。");
return;
}
var location = DetourInterface.getCartLocation();
if (double.IsNaN(location.x) ||
double.IsInfinity(location.x) ||
double.IsNaN(location.y) ||
double.IsInfinity(location.y) ||
double.IsNaN(location.th) ||
double.IsInfinity(location.th))
{
Console.WriteLine(
"Detour当前位姿无效，取消原地旋转测试。");
return;
}
var rotationCenter =
new Vector2((float)location.x, (float)location.y);
var targetWorldAngle =
NormalizeDegrees(
(float)location.th + RelativeAngleDegrees);
_recorder = new TrackingExperimentRecorder(
controllerName: "InPlaceRotatePID",
trajectoryName: "Rotate90",
trialNumber: TrialNumber,
referenceStart: rotationCenter,
referenceEnd: rotationCenter,
referenceSpeed:
MaxAngularSpeedDegreesPerSecond);
_recorder.Start();
try
{
_task = new DriveTask(
new MultiWheelRotateInPlace
{
// MultiWheelRotateInPlace接收世界坐标系绝对航向。
AngleTarget = targetWorldAngle,
PidparamsRead = () => new PIDParams
{
Kp =
PilotDefinition.Conf.InPlaceRotateKp,
Ki =
PilotDefinition.Conf.InPlaceRotateKi,
Kd =
PilotDefinition.Conf.InPlaceRotateKd,
DeadZone =
PilotDefinition.Conf
.InPlaceRotateArriveDeg,
SpeedAccPerSec =
PilotDefinition.Conf.InPlaceRotateAcc,
OutputUpperThreshold =
MaxAngularSpeedDegreesPerSecond,
MaxI =
PilotDefinition.Conf.InPlaceRotateMaxI
},
CommandAngularSpeedObserver =
commandAngularSpeed =>
_recorder?.UpdateCommand(
0f,
commandAngularSpeed)
}.Get());
_task.Wait();
// 保留少量停止后的样本，用于观察角速度是否回到零。
Thread.Sleep(300);
}
finally
{
_task?.Stop();
_recorder?.UpdateCommand(0f, 0f);
_recorder?.StopAndSave();
_task = null;
_recorder = null;
}
}
// 停止原地旋转并保存当前已经采集的实验数据。
public override void TestStop()
{
_task?.Stop();
_recorder?.UpdateCommand(0f, 0f);
_recorder?.StopAndSave();
}
// 将世界航向归一化到大约[-180°,180°]。
private static float NormalizeDegrees(float angleDegrees)
{
return (float)(
angleDegrees -
Math.Round(angleDegrees / 360.0) * 360.0);
}
}
[MovementTest(name = "测试左转90°圆弧")]
public class TestArcMovement : MovementTest
{
public float RadiusMillimeters = 2000f; // 左转圆的半径，单位mm。
public float CruiseSpeed = 0.3f; // 圆周运动速度上限，单位m/s。
public int TrialNumber = 1; // 重复实验编号。
private DriveTask _task;
private TrackingExperimentRecorder _recorder;
// 从当前位姿开始，沿半径2m的圆弧向左转弯90°。
public override void Test()
{
if (float.IsNaN(RadiusMillimeters) ||
float.IsInfinity(RadiusMillimeters) ||
RadiusMillimeters <= 0f ||
float.IsNaN(CruiseSpeed) ||
float.IsInfinity(CruiseSpeed) ||
CruiseSpeed <= 0f)
{
Console.WriteLine("圆弧运动测试参数无效。");
return;
}
if (!MovementTestPreparation.AreWheelsForward())
{
return;
}
var location = DetourInterface.getCartLocation();
if (double.IsNaN(location.x) ||
double.IsInfinity(location.x) ||
double.IsNaN(location.y) ||
double.IsInfinity(location.y) ||
double.IsNaN(location.th) ||
double.IsInfinity(location.th))
{
Console.WriteLine(
"Detour当前位姿无效，取消圆弧运动测试。");
return;
}
var source =
new Vector2((float)location.x, (float)location.y);
var headingRadians =
location.th * Math.PI / 180.0;
// 根据世界航向求车体左法向，左转圆心位于车辆左侧。
var center = new Vector2(
source.X -
RadiusMillimeters *
(float)Math.Sin(headingRadians),
source.Y +
RadiusMillimeters *
(float)Math.Cos(headingRadians));
// 从圆心指向车辆起点的极角，比车辆切线航向小90°。
var startRadialAngleDegrees =
(float)location.th - 90f;
var controller = new ChassisController
{
BaseSpeed = CruiseSpeed
}.Get();
controller.FinishSpeed = 0f;
var arc = new CircularArcTrack(
center,
RadiusMillimeters,
startRadialAngleDegrees,
startRadialAngleDegrees + 90f,
direction: 1)
{
Speed = CruiseSpeed,
CarDirectionBias = 0f
};
if (!controller.AddTrack(arc, "LeftArc90Degrees"))
{
Console.WriteLine(
"左转90°圆弧轨迹添加失败，取消测试。");
return;
}
_recorder = new TrackingExperimentRecorder(
controllerName: "GeometricController",
trajectoryName:
$"LeftArc90_R{RadiusMillimeters:0}mm",
trialNumber: TrialNumber,
referenceStart: source,
referenceEnd: source,
referenceSpeed: CruiseSpeed);
_recorder.Start();
try
{
_task = new DriveTask(controller.Track());
_task.Wait();
// 保留少量停车后的样本，用于观察速度是否回到零。
Thread.Sleep(300);
}
finally
{
_task?.Stop();
_recorder?.UpdateCommand(0f, 0f);
_recorder?.StopAndSave();
_task = null;
_recorder = null;
}
}
// 停止圆弧运动并保存当前已经采集的实验数据。
public override void TestStop()
{
_task?.Stop();
_recorder?.UpdateCommand(0f, 0f);
_recorder?.StopAndSave();
}
}
public abstract class ClampMovementTestBase : MovementTest
{
public float TimeoutSeconds = 30f; // 动作超时时间，单位s。
private DriveTask _task;
protected abstract bool Close { get; }
// 根据派生测试类型驱动左右夹臂同步夹紧或打开。
public override void Test()
{
var leftTarget = Close
? PilotDefinition.Self.LeftArmUpperPos
: PilotDefinition.Self.LeftArmLowerPos;
var rightTarget = Close
? PilotDefinition.Self.RightArmUpperPos
: PilotDefinition.Self.RightArmLowerPos;
if (float.IsNaN(leftTarget) ||
float.IsInfinity(leftTarget) ||
float.IsNaN(rightTarget) ||
float.IsInfinity(rightTarget))
{
Console.WriteLine(
"夹臂目标位置无效，取消夹臂运动测试。");
return;
}
// 防止重复点击时上一项夹臂任务仍在运行。
TestStop();
Console.WriteLine(
$"开始夹臂{(Close ? "夹紧" : "打开")}测试：" +
$"左目标={leftTarget}，右目标={rightTarget}");
var task = new DriveTask(
new ClampToTarget
{
LeftClampTarget = leftTarget,
RightClampTarget = rightTarget,
TimeoutSeconds = TimeoutSeconds
}.Get());
_task = task;
try
{
task.Wait();
}
finally
{
// 防止旧任务结束时，错误清除后来启动的新任务。
if (ReferenceEquals(_dt, task))
{
_dt = null;
}
PilotDefinition.Self.SpeedLeftArm = 0f;
PilotDefinition.Self.SpeedRightArm = 0f;
if (ReferenceEquals(_task, task))
_task = null;
}
}
// 停止夹臂任务并立即清零左右夹臂下发速度。
public override void TestStop()
{
_task?.Stop();
_task = null;
PilotDefinition.Self.SpeedLeftArm = 0f;
PilotDefinition.Self.SpeedRightArm = 0f;
}
}
[MovementTest(name = "夹臂关闭测试")]
public sealed class TestClampOpenMovement
: ClampMovementTestBase
{
protected override bool Close => false;
}
[MovementTest(name = "夹臂启动测试")]
public sealed class TestClampCloseMovement
: ClampMovementTestBase
{
protected override bool Close => true;
}
}
#region 旧版测试
// public abstract class DstTrackerTestBase : MovementTest
// {
// public bool UseInteractivePick = true;
// public float srcX;
// public float srcY;
// public float dstX;
// public float dstY;
// public float carDirectionBias;
// private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
// private DriveTask _dt;
// protected DstTrackerTestBase(float defaultCarDirectionBias)
// {
// carDirectionBias = defaultCarDirectionBias;
// }
// public override void TestStop()
// {
// _dt?.Stop();
// _painter?.Clear();
// }
// public override void Test()
// {
// Vector2 p1;
// Vector2 p2;
// if (UseInteractivePick)
// {
// p1 = UI.GetPoint("point1");
// p2 = UI.GetPoint("point2");
// }
// else
// {
// p1 = new Vector2(srcX, srcY);
// p2 = new Vector2(dstX, dstY);
// }
// _painter.Clear();
// _dt = new DriveTask(new DstTracker
// {
// Src = p1,
// Dst = p2,
// CarDirectionBias = carDirectionBias,
// }.Get());
// _dt.Wait();
// }
// }
// [MovementTest(name = "测试终点跟踪动作-前进")]
// public sealed class DstTrackerForward : DstTrackerTestBase
// {
// public DstTrackerForward() : base(0f) { }
// }
// [MovementTest(name = "测试终点跟踪动作-后退")]
// public sealed class DstTrackerBackward : DstTrackerTestBase
// {
// public DstTrackerBackward() : base(180f) { }
// }
// [MovementTest(name = "底盘旋转测试")]
// public class RotateToAngleTest : MovementTest
// {
// private DriveTask _dt;
// // 停止当前正在执行的底盘原地旋转任务。
// public override void TestStop()
// {
// _dt?.Stop();
// }
// // 交互输入目标角度后执行底盘原地旋转测试。
// public override void Test()
// {
// var input = UI.GetInput("输入旋转角度：");
// if (!float.TryParse(input, out var angleTarget))
// {
// Console.WriteLine(
// $"旋转测试输入无效：{input}");
// return;
// }
// // 防止重复启动测试时，上一项旋转任务仍在运行。
// _dt?.Stop();
// var task = new DriveTask(
// new MultiWheelRotateInPlace
// {
// AngleTarget = angleTarget,
// PidparamsRead = () => new PIDParams
// {
// Kp = PilotDefinition.Conf.InPlaceRotateKp,
// Ki = PilotDefinition.Conf.InPlaceRotateKi,
// Kd = PilotDefinition.Conf.InPlaceRotateKd,
// DeadZone = PilotDefinition.Conf.InPlaceRotateArriveDeg,
// SpeedAccPerSec = PilotDefinition.Conf.InPlaceRotateAcc,
// OutputUpperThreshold = PilotDefinition.Conf.InPlaceRotateMaxSpeed,
// MaxI = PilotDefinition.Conf.InPlaceRotateMaxI,
// }
// }.Get());
// _dt = task;
// try
// {
// task.Wait();
// }
// finally
// {
// // 防止旧任务结束时，错误清除后来启动的新任务。
// if (ReferenceEquals(_dt, task))
// {
// _dt = null;
// }
// }
// }
// }
#endregion
ClumsyPilot/Movements.cs
+457 -23
View File
@@ -11,44 +11,92 @@ using System.Collections.Generic;
using System.Drawing;
using System.Numerics;
using System.Threading;
using FundamentalLib;
using MyParking.Shared;
namespace MultiWheelC
{
public class DstTracker : MovementDefinition
// C层测试准备：停车并等待四个舵轮稳定回到车体前向0°。
public class PrepareWheelsForward : MovementDefinition
{
public Vector2 Src;
public Vector2 Dst;
public float CarDirectionBias = 0f;
public Painter Painter = UI.GetPainter("DstTracker");
public float ToleranceDegrees = 2f;
public float StableSeconds = 0.3f;
public float TimeoutSeconds = 10f;
public bool Completed { get; private set; }
public override IEnumerable<bool> Get()
{
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
DriveTask task = null;
var chassis =
PilotDefinition.Chassis as MultiWheelChassis;
if (chassis == null)
{
throw new InvalidOperationException(
"当前底盘不是MultiWheelChassis，无法执行舵轮回正。");
}
var adapter = new MultiWheelChassisAdapter(
chassis,
PilotDefinition.Self.CarNum);
var toleranceRadians =
ToleranceDegrees * Math.PI / 180.0;
var startTime = DateTime.UtcNow;
DateTime? alignedSince = null;
Completed = false;
if (!adapter.PrepareParallelDirection(0.0))
{
throw new InvalidOperationException(
"无法将所有舵轮下发到车体前向0°。");
}
try
{
Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
var tracker = new ChassisController().Get();
var linePath = new LineTrack(Src, Dst)
while (true)
{
CarDirectionBias = CarDirectionBias,
Speed = PilotDefinition.Conf.DstTrackerMaxSpeed
};
tracker.AddTrack(linePath);
task = new DriveTask(tracker.Track());
task.Wait();
yield return false;
var aligned =
adapter.AreParallelWheelsAligned(
0.0,
toleranceRadians);
if (aligned)
{
if (!alignedSince.HasValue)
alignedSince = DateTime.UtcNow;
if ((DateTime.UtcNow -
alignedSince.Value).TotalSeconds >=
StableSeconds)
{
Completed = true;
yield break;
}
}
else
{
alignedSince = null;
}
if (TimeoutSeconds > 0f &&
(DateTime.UtcNow - startTime).TotalSeconds >
TimeoutSeconds)
{
throw new TimeoutException(
$"舵轮回正超过{TimeoutSeconds:F1}s，" +
"测试已经取消。");
}
yield return true;
}
}
finally
{
task?.Stop();
chassis.SendXYThSpeed(0f, 0f, 0f);
// 只清零驱动速度，保留已经下发的0°舵角。
adapter.StopImmediately();
}
}
}
#region 功能项
public class Sleep : MovementDefinition
{
public float Second = 2f;
@@ -71,7 +119,303 @@ namespace MultiWheelC
yield return false;
}
}
public class DriverAble : MovementDefinition
{
public int WaitTimeoutMs = 2000;
public int PollIntervalMs = 50;
// C层单车硬件：请求全部驱动轮复位并恢复使能。
public override IEnumerable<bool> Get()
{
PilotDefinition.Self.ResetFromC = true;
try
{
var start = DateTime.Now;
var timeoutMs = Math.Max(0, WaitTimeoutMs);
var pollMs = Math.Max(1, PollIntervalMs);
// 至少保留一个调度周期，确保M层能收到复位请求。
yield return true;
while (!PilotDefinition.Self.WheelAbleState &&
(DateTime.Now - start).TotalMilliseconds < timeoutMs)
{
Thread.Sleep(pollMs);
yield return true;
}
}
finally
{
PilotDefinition.Self.ResetFromC = false;
}
}
}
public class DriverDisable : MovementDefinition
{
public int WaitTimeoutMs = 3000;
public int PollIntervalMs = 20;
// C层单车硬件：请求驱动轮退出使能，并等待M层状态反馈。
public override IEnumerable<bool> Get()
{
var timeoutMs = Math.Max(0, WaitTimeoutMs);
var pollMs = Math.Max(1, PollIntervalMs);
var startTime = DateTime.UtcNow;
var success = false;
PilotDefinition.Self.DisableFromC = true;
try
{
// 至少保持一个C层调度周期，确保M层能收到下使能请求。
yield return true;
success = !PilotDefinition.Self.WheelAbleState;
while (!success &&
(DateTime.UtcNow - startTime).TotalMilliseconds <
timeoutMs)
{
Thread.Sleep(pollMs);
success =
!PilotDefinition.Self.WheelAbleState;
if (!success)
{
yield return true;
}
}
}
finally
{
// 无论正常完成、超时、异常还是任务被停止，都撤销请求。
PilotDefinition.Self.DisableFromC = false;
}
if (success)
{
Console.WriteLine(
$"驱动器下使能完成，" +
$"WheelAbleState=" +
$"{PilotDefinition.Self.WheelAbleState}");
}
else
{
Console.WriteLine(
$"驱动器下使能超时，" +
$"WheelAbleState=" +
$"{PilotDefinition.Self.WheelAbleState}，" +
$"等待{timeoutMs}ms");
}
yield return false;
}
}
#endregion
#region 直线运动
//在世界坐标系下，从路径起点追踪到终点并停车
public class DstTracker : MovementDefinition
{
public Vector2 Src;
public Vector2 Dst;
// 本次轨迹的巡航速度上限，单位m/s。
public float MaxSpeed = PilotDefinition.Conf.DstTrackerMaxSpeed;
public float CarDirectionBias = 0f;
public Painter Painter = UI.GetPainter("DstTracker");
public override IEnumerable<bool> Get()
{
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
DriveTask task = null;
try
{
Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
var tracker = new ChassisController
{
BaseSpeed = MaxSpeed
}.Get();
// 要求路径末端速度下降到零。
tracker.FinishSpeed = 0f;
var linePath = new LineTrack(Src, Dst)
{
CarDirectionBias = CarDirectionBias,
Speed = MaxSpeed
};
tracker.AddTrack(linePath);
task = new DriveTask(tracker.Track());
task.Wait();
yield return false;
}
finally
{
task?.Stop();
chassis.SendXYThSpeed(0f, 0f, 0f);
}
}
}
//直线行走基于轮里程
// C层单车底盘：按照车轮里程行驶指定的相对距离。
public class LineTracking : MovementDefinition
{
// 相对动作启动位置的行驶距离，单位mm。
// 正数表示前进，负数表示后退。
public float TargetDistance;
public float MaxSpeed = PilotDefinition.Conf.LineTrackMaxSpeed;
public float Kp = PilotDefinition.Conf.LineTrackKp;
public float Ki = PilotDefinition.Conf.LineTrackKi;
public float Kd = PilotDefinition.Conf.LineTrackKd;
public float DeadZone = PilotDefinition.Conf.LineTrackDeadZone;
public int SrcId = -1;
public int DstId = -1;
public Action<int> LeaveSrcFunction;
// 接近目标后是否保留速度，交给下一个动作接管。
public bool EnableHandover;
// 进入动作衔接的剩余距离，单位mm。
public float HandoverDistance = 80f;
// HandoverSpeed小于0时，使用MaxSpeed的此比例。
public float HandoverSpeedRatio = 0.5f;
// 大于等于0时，直接作为衔接速度，单位m/s。
public float HandoverSpeed = -1f;
public float MinHandoverSpeed = 0.05f;
private PIDController _pid;
// 读取当前单车直线行驶里程，单位mm。
private static float ReadPosition()
{
return
(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2f;
}
// 根据动作启动位置和目标距离执行直线里程闭环。
public override IEnumerable<bool> Get()
{
if (float.IsNaN(TargetDistance) || float.IsInfinity(TargetDistance))
{
throw new ArgumentOutOfRangeException(
nameof(TargetDistance),
"目标行驶距离必须是有限值。");
}
if (float.IsNaN(MaxSpeed) || float.IsInfinity(MaxSpeed) || MaxSpeed <= 0f)
{
throw new ArgumentOutOfRangeException(
nameof(MaxSpeed),
"最大速度必须是大于零的有限值。");
}
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
// 每次启动动作时重新读取起始编码器位置。
var startPosition = ReadPosition();
// PID仍然控制绝对编码器位置，但绝对目标由动作自动计算。
var targetPosition = startPosition + TargetDistance;
_pid = new PIDController(ReadPosition, Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
{
SpeedAccPerSec = Math.Abs(MaxSpeed) / 2f
};
var handoverRequested = false;
var keepHandoverSpeed = false;
DLog.Log(
$"直线里程动作：" +
$"起点={startPosition:F1}mm，" +
$"距离={TargetDistance:F1}mm，" +
$"目标={targetPosition:F1}mm",
"straight_line");
try
{
while (true)
{
var currentPosition = ReadPosition();
var remainingDistance = targetPosition - currentPosition;
// 接近目标后，保留一定速度交给后续动作。
if (EnableHandover && Math.Abs(remainingDistance) <= Math.Max(1f, HandoverDistance))
{
var direction = Math.Sign(remainingDistance);
if (direction == 0)
{
direction = Math.Sign(TargetDistance);
}
var requestedSpeed = HandoverSpeed >= 0f ? Math.Abs(HandoverSpeed) : Math.Abs(MaxSpeed) * HandoverSpeedRatio;
var maximumSpeed = Math.Abs(MaxSpeed);
var minimumSpeed = Math.Min(Math.Abs(MinHandoverSpeed), maximumSpeed);
var limitedSpeed = Math.Max(minimumSpeed, Math.Min(requestedSpeed, maximumSpeed));
var handoverSpeed = limitedSpeed * direction;
chassis.SendXYThSpeed(handoverSpeed, 0f, 0f);
handoverRequested = true;
// 保持一个调度周期，让速度命令实际生效。
yield return true;
break;
}
var speed = _pid.GetResponse(targetPosition);
chassis.SendXYThSpeed(speed, 0f, 0f);
if (_pid.IsArrived())
{
break;
}
yield return true;
}
if (SrcId != -1 &&
LeaveSrcFunction != null)
{
LeaveSrcFunction(SrcId);
DLog.Log($"释放放车点{SrcId}", "straight_line");
}
// 只有正常完成动作衔接时才允许保留非零速度。
keepHandoverSpeed = handoverRequested;
}
finally
{
// 普通完成、人工停止或异常退出时都必须停车。
if (!keepHandoverSpeed)
{
chassis.SendXYThSpeed(0f, 0f, 0f);
}
}
yield return false;
}
}
//直线行走基于detour
public class LineTracking_based_detour : MovementDefinition
{
public float LineDistance = 1000f;
public int SrcId = -1;
public int DstId = -1;
public Action<int> LeaveSrcFunction = null;
public Painter painter = UI.GetPainter("Line", false);
// C层单车轨迹：执行早期版本的两点直线跟踪动作。
public override IEnumerable<bool> Get()
{
var curpose = DetourInterface.getCartLocation();
Console.WriteLine($"curpose.th:{curpose.th}");
var src = new Vector2((float)curpose.x, (float)curpose.y);
var headingRadians = curpose.th * Math.PI / 180.0;
var dst = new Vector2(
(float)(curpose.x +
LineDistance * Math.Cos(headingRadians)),
(float)(curpose.y +
LineDistance * Math.Sin(headingRadians)));
// var dst = new Vector2((float)curpose.x + LineDistance * (float)Math.Cos(curpose.th),
// (float)curpose.y + LineDistance * (float)Math.Sin(curpose.th));
Console.WriteLine($"src:{src.X} {src.Y}");
Console.WriteLine($"dst:{dst.X} {dst.Y}");
painter.DrawLine(Color.Green, src.X, src.Y, dst.X, dst.Y, width: 3);
var tracker = new ChassisController().Get();
var linePath = new LineTrack(src, dst) { CarDirectionBias = LineDistance > 0 ? 0 : 180 };
tracker.AddTrack(linePath);
var _dt = new DriveTask(tracker.Track());
_dt.Wait();
if (SrcId != -1 && LeaveSrcFunction != null)
{
LeaveSrcFunction(SrcId);
DLog.Log($"释放放车点{SrcId}", "straight_line");
}
yield return false;
}
}
#endregion
#region 旋转运动
public class MultiWheelRotateInPlace : MovementDefinition
{
/// <summary>
@@ -89,7 +433,10 @@ namespace MultiWheelC
public PIDController thPid;
// 将角度归一化到零到三百六十度范围内。
// 将本周期PID角速度输出提供给实验记录器，单位deg/s。
public Action<float> CommandAngularSpeedObserver;
// 归一化到大约 [-180°, 180°]
private static float RangeAngle(float theta)
{
return (float)(theta - Math.Round(theta / 360.0f) * 360);
@@ -110,6 +457,7 @@ namespace MultiWheelC
{
var s = thPid.GetResponse(targetAngle, true);
Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
CommandAngularSpeedObserver?.Invoke(s);
Chassis.SendXYThSpeed(0, 0, s);
if (thPid.IsArrived()) break;
yield return true;
@@ -119,10 +467,96 @@ namespace MultiWheelC
}
finally
{
CommandAngularSpeedObserver?.Invoke(0f);
Chassis.SendXYThSpeed(0, 0, 0);
}
}
}
#endregion
#region 夹臂运动
public class ClampToTarget : MovementDefinition
{
public float LeftClampTarget;
public float RightClampTarget;
public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed;
public float ClampKp = PilotDefinition.Conf.ClampControlKp;
public float ClampKi = PilotDefinition.Conf.ClampControlKi;
public float ClampKd = PilotDefinition.Conf.ClampControlKd;
public float ClampMaxI = PilotDefinition.Conf.ClampControlMaxI;
public float ClampSpeedAcc = PilotDefinition.Conf.ClampControlSpeedAcc;
public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
public float TimeoutSeconds = 30f;
private PIDController leftpid, rightpid;
// C层单车业务：驱动左右夹臂运动到夹紧或松开目标。
public override IEnumerable<bool> Get()
{
try
{
leftpid = new PIDController(
() => PilotDefinition.Self.ActualPosLeftArm,
ClampKp, ClampKi, ClampKd, ClampMaxI,
ClampDeadZone, MaxClampSpeed)
{
SpeedAccPerSec = ClampSpeedAcc
};
rightpid = new PIDController(
() => PilotDefinition.Self.ActualPosRightArm,
ClampKp, ClampKi, ClampKd, ClampMaxI,
ClampDeadZone, MaxClampSpeed)
{
SpeedAccPerSec = ClampSpeedAcc
};
var startTime = DateTime.UtcNow;
while (true)
{
if (TimeoutSeconds > 0f &&
(DateTime.UtcNow - startTime).TotalSeconds >
TimeoutSeconds)
{
Console.WriteLine(
$"夹臂运动超时({TimeoutSeconds:F1}s)，" +
"停止左右夹臂。");
yield break;
}
var leftspeed =
leftpid.GetResponse(LeftClampTarget);
var rightspeed =
rightpid.GetResponse(RightClampTarget);
Console.WriteLine(
$"left arm speed:{leftspeed} " +
$"right arm speed:{rightspeed}");
PilotDefinition.Self.SpeedLeftArm = leftspeed;
PilotDefinition.Self.SpeedRightArm = rightspeed;
var leftArrived = leftpid.IsArrived();
var rightArrived = rightpid.IsArrived();
if (leftArrived)
PilotDefinition.Self.SpeedLeftArm = 0f;
if (rightArrived)
PilotDefinition.Self.SpeedRightArm = 0f;
if (leftArrived && rightArrived)
break;
yield return true;
}
Console.WriteLine(
$"left clamp to target:{LeftClampTarget} " +
$"right clamp to target:{RightClampTarget}");
}
finally
{
PilotDefinition.Self.SpeedLeftArm = 0f;
PilotDefinition.Self.SpeedRightArm = 0f;
}
}
}
#endregion
}
ClumsyPilot/PilotConfig.cs
+1 -1
View File
@@ -38,7 +38,7 @@ public class PilotConfig : MultiWheelPilotConfig
#region 单车-临时
[FieldMember(desc = "原地旋转Kp")]
public float InPlaceRotateKp = 0.05f;
public float InPlaceRotateKp = 0.2f;
[FieldMember(desc = "原地旋转Ki")]
public float InPlaceRotateKi = 0.01f;
ClumsyPilot/TrackingExperimentRecorder.cs
+394
View File
@@ -0,0 +1,394 @@
using ClumsyCore.Interfaces;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Globalization;
using System.IO;
using System.Numerics;
using System.Text;
using System.Threading;
namespace MultiWheelC
{
// C层实验数据：保存一个采样时刻的定位与控制命令。
public sealed class TrackingSample
{
public double ElapsedSeconds;
// Detour位置单位为mm，航向单位为deg。
public double DetourX;
public double DetourY;
public double DetourTheta;
// 车体速度单位为m/s，角速度单位为deg/s。
public float CommandSpeed;
public float CommandVx;
public float CommandVy;
public float CommandAngularSpeed;
}
// C层实验工具：统一采集并保存轨迹跟踪实验数据。
public sealed class TrackingExperimentRecorder
{
private readonly string _controllerName;
private readonly string _trajectoryName;
private readonly int _trialNumber;
private readonly Vector2 _referenceStart;
private readonly Vector2 _referenceEnd;
private readonly float _referenceSpeed;
private readonly int _sampleIntervalMs;
private readonly List<TrackingSample> _samples =
new List<TrackingSample>();
private readonly object _sampleSyncRoot =
new object();
private readonly object _commandSyncRoot =
new object();
private readonly Stopwatch _stopwatch =
new Stopwatch();
private Thread _worker;
private volatile bool _running;
private int _started;
private int _saved;
private bool _hasExternalCommand;
private float _externalCommandSpeed;
private float _externalCommandVx;
private float _externalCommandVy;
private float _externalCommandAngularSpeed;
public TrackingExperimentRecorder(
string controllerName,
string trajectoryName,
int trialNumber,
Vector2 referenceStart,
Vector2 referenceEnd,
float referenceSpeed,
int sampleIntervalMs = 50)
{
if (string.IsNullOrWhiteSpace(controllerName))
throw new ArgumentException(
"控制器名称不能为空。",
nameof(controllerName));
if (string.IsNullOrWhiteSpace(trajectoryName))
throw new ArgumentException(
"轨迹名称不能为空。",
nameof(trajectoryName));
if (sampleIntervalMs <= 0)
throw new ArgumentOutOfRangeException(
nameof(sampleIntervalMs),
"采样周期必须大于零。");
_controllerName = controllerName;
_trajectoryName = trajectoryName;
_trialNumber = trialNumber;
_referenceStart = referenceStart;
_referenceEnd = referenceEnd;
_referenceSpeed = referenceSpeed;
_sampleIntervalMs = sampleIntervalMs;
}
// 保存成功后的CSV绝对路径；尚未保存时为空。
public string SavedFilePath { get; private set; }
// 启动后台采样线程。
public void Start()
{
if (Interlocked.Exchange(ref _started, 1) != 0)
return;
_stopwatch.Restart();
_running = true;
// 立即保存起点静止状态，避免第一帧被后台线程延迟。
CaptureSample();
_worker = new Thread(SamplingLoop)
{
IsBackground = true,
Name = "TrackingExperimentRecorder"
};
_worker.Start();
}
// 供Stanley/LQR控制器主动写入本周期最终速度命令。
// 调用后优先记录该命令，不再使用底盘反解值。
public void UpdateCommand(
float commandSpeed,
float commandAngularSpeed)
{
lock (_commandSyncRoot)
{
_externalCommandSpeed = commandSpeed;
_externalCommandVx = commandSpeed;
_externalCommandVy = 0f;
_externalCommandAngularSpeed =
commandAngularSpeed;
_hasExternalCommand = true;
}
}
// 供全向、蟹行和曲线控制器写入完整车体速度命令。
public void UpdateBodyCommand(
float commandVx,
float commandVy,
float commandAngularSpeed)
{
lock (_commandSyncRoot)
{
_externalCommandVx = commandVx;
_externalCommandVy = commandVy;
_externalCommandSpeed =
(float)Math.Sqrt(
commandVx * commandVx +
commandVy * commandVy);
_externalCommandAngularSpeed =
commandAngularSpeed;
_hasExternalCommand = true;
}
}
// 停止采样并将本次实验保存为CSV；重复调用只保存一次。
public void StopAndSave()
{
if (Volatile.Read(ref _started) == 0)
return;
if (Interlocked.Exchange(ref _saved, 1) != 0)
return;
try
{
_running = false;
if (_worker != null &&
_worker != Thread.CurrentThread)
{
_worker.Join(
Math.Max(1000, _sampleIntervalMs * 4));
}
// 保存停止时刻的最后一帧。
CaptureSample();
_stopwatch.Stop();
SaveCsv();
Console.WriteLine(
$"轨迹实验数据已保存：{SavedFilePath}");
}
catch
{
// 保存失败后允许调用者再次尝试。
Interlocked.Exchange(ref _saved, 0);
throw;
}
}
// 按固定周期采集Detour位姿和控制命令。
private void SamplingLoop()
{
while (_running)
{
Thread.Sleep(_sampleIntervalMs);
if (!_running)
break;
CaptureSample();
}
}
// 采集一帧Detour位姿和控制命令。
private void CaptureSample()
{
try
{
var location =
DetourInterface.getCartLocation();
float commandSpeed;
float commandVx;
float commandVy;
float commandAngularSpeed;
lock (_commandSyncRoot)
{
if (_hasExternalCommand)
{
commandSpeed =
_externalCommandSpeed;
commandVx =
_externalCommandVx;
commandVy =
_externalCommandVy;
commandAngularSpeed =
_externalCommandAngularSpeed;
}
else
{
var command =
PilotDefinition.Chassis
.GetCarSpeed(false);
commandVx = command.Vx;
commandVy = command.Vy;
commandAngularSpeed = command.Vw;
commandSpeed = (float)Math.Sqrt(
commandVx * commandVx +
commandVy * commandVy);
}
}
var sample = new TrackingSample
{
ElapsedSeconds =
_stopwatch.Elapsed.TotalSeconds,
DetourX = location.x,
DetourY = location.y,
DetourTheta = location.th,
CommandSpeed = commandSpeed,
CommandVx = commandVx,
CommandVy = commandVy,
CommandAngularSpeed =
commandAngularSpeed
};
lock (_sampleSyncRoot)
{
_samples.Add(sample);
}
}
catch (Exception ex)
{
// 单帧读取失败不应终止车辆控制或整个记录线程。
Console.WriteLine(
$"轨迹实验采样失败：{ex.Message}");
}
}
// 将内存中的采样数据写入CSV。
private void SaveCsv()
{
List<TrackingSample> snapshot;
lock (_sampleSyncRoot)
{
snapshot =
new List<TrackingSample>(_samples);
}
var outputDirectory = Path.Combine(
AppContext.BaseDirectory,
"TrackingExperiments");
Directory.CreateDirectory(outputDirectory);
var fileName =
$"{DateTime.Now:yyyyMMdd_HHmmss_fff}_" +
$"{SanitizeFileName(_controllerName)}_" +
$"{SanitizeFileName(_trajectoryName)}_" +
$"Trial{_trialNumber}.csv";
SavedFilePath = Path.Combine(
outputDirectory,
fileName);
using (var writer = new StreamWriter(
SavedFilePath,
false,
new UTF8Encoding(true)))
{
writer.WriteLine(
"ElapsedSeconds," +
"ControllerName," +
"TrajectoryName," +
"TrialNumber," +
"DetourX," +
"DetourY," +
"DetourTheta," +
"CommandSpeed," +
"CommandAngularSpeed," +
"CommandVx," +
"CommandVy," +
"ReferenceStartX," +
"ReferenceStartY," +
"ReferenceEndX," +
"ReferenceEndY," +
"ReferenceSpeed");
foreach (var sample in snapshot)
{
writer.WriteLine(string.Join(
",",
Format(sample.ElapsedSeconds),
EscapeCsv(_controllerName),
EscapeCsv(_trajectoryName),
_trialNumber.ToString(
CultureInfo.InvariantCulture),
Format(sample.DetourX),
Format(sample.DetourY),
Format(sample.DetourTheta),
Format(sample.CommandSpeed),
Format(sample.CommandAngularSpeed),
Format(sample.CommandVx),
Format(sample.CommandVy),
Format(_referenceStart.X),
Format(_referenceStart.Y),
Format(_referenceEnd.X),
Format(_referenceEnd.Y),
Format(_referenceSpeed)));
}
}
}
// 将文件名中的非法字符替换为下划线。
private static string SanitizeFileName(string value)
{
var result = value;
foreach (var invalidCharacter in
Path.GetInvalidFileNameChars())
{
result = result.Replace(
invalidCharacter,
'_');
}
return result;
}
// 按固定小数格式输出数值，避免系统区域设置改变CSV格式。
private static string Format(double value)
{
return value.ToString(
"0.######",
CultureInfo.InvariantCulture);
}
// 对CSV文本字段进行引号和逗号转义。
private static string EscapeCsv(string value)
{
if (value == null)
return string.Empty;
if (!value.Contains(",") &&
!value.Contains("\"") &&
!value.Contains("\r") &&
!value.Contains("\n"))
{
return value;
}
return
"\"" +
value.Replace("\"", "\"\"") +
"\"";
}
}
}
ClumsyPilot/build/Clumsy/MultiWheelC.dll
BIN
View File
Binary file not shown.
ClumsyPilot/build/Clumsy/MultiWheelC.pdb
BIN
View File
Binary file not shown.
ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfo.cs
+1 -1
View File
@@ -13,7 +13,7 @@ using System.Reflection;
[assembly: System.Reflection.AssemblyCompanyAttribute("ClumsyPilot")]
[assembly: System.Reflection.AssemblyConfigurationAttribute("Debug")]
[assembly: System.Reflection.AssemblyFileVersionAttribute("1.0.0.0")]
[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0+580a936a830dcb7a25ef327cf553341405264033")]
[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0+e6b99c45b352f24ff58092f7855d0eadd8828d42")]
[assembly: System.Reflection.AssemblyProductAttribute("ClumsyPilot")]
[assembly: System.Reflection.AssemblyTitleAttribute("ClumsyPilot")]
[assembly: System.Reflection.AssemblyVersionAttribute("1.0.0.0")]
ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfoInputs.cache
+1 -1
View File
@@ -1 +1 @@
038d57c5b1714403d308c9343b385ef762951607b63a9fb275f4d7d2b8fd29cb
3e5507bf56e38facdc245d5ff407e181c060d8094f38b94ad743b8c3305523f2
ClumsyPilot/obj/Debug/ClumsyPilot.csproj.CoreCompileInputs.cache
+1 -1
View File
@@ -1 +1 @@
3920568398d269aea7b71fb4581ad111777c15ca3f2f2bb272ee9e0a989215c4
e5afef3021b83202643b31f468b83ac62f224cd367fe52dc0a1cbb98570c91e1
ClumsyPilot/obj/Debug/ClumsyPilot.dll
BIN
View File
Binary file not shown.
ClumsyPilot/obj/Debug/ClumsyPilot.pdb
BIN
View File
Binary file not shown.