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

完善蟹行虚拟阿克曼与SendMotion运动坐标系，并添加轮速诊断日志

Browse Source 
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
yuxiang.shen
2026-07-30 18:10:22 +08:00
co-authored by Cursor
parent 7e05ff098e
commit c1fe73caec
92 changed files with 1794 additions and 776 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/CrabMotionFrameTracker.cs
+231 -25
View File
@@ -20,7 +20,16 @@ namespace MultiWheelC
SCurve = 2
}
public enum ChassisCommandBackend
{
SendXYThSpeed = 0,
SendMotion = 1,
VirtualAckermann = 2
}
public ReferencePathKind PathKind;
public ChassisCommandBackend CommandBackend =
ChassisCommandBackend.SendMotion;
public Vector2 StartPosition;
public double InitialBodyYawRadians;
public float LengthMillimeters = 4000f;
@@ -36,6 +45,8 @@ namespace MultiWheelC
public double HeadingGainPerSecond = 1.5;
public double MaximumAngularSpeedRadiansPerSecond =
30.0 * Math.PI / 180.0;
public double MaximumVirtualSteeringRadians =
30.0 * Math.PI / 180.0;
public float WheelAlignmentToleranceDegrees = 2f;
public float WheelAlignmentStableSeconds = 0.3f;
public float WheelAlignmentTimeoutSeconds = 10f;
@@ -104,6 +115,15 @@ namespace MultiWheelC
yield return true;
}
if (CommandBackend ==
ChassisCommandBackend.SendMotion)
{
// 舵轮已按真实机械角度完成预对齐；
// 现在由Shared适配层激活SendMotion虚拟运动坐标系。
adapter.ActivateMotionFrame(
MotionFrameYawInBodyRadians);
}
var trackingStarted = DateTime.Now;
while (true)
{
@@ -179,8 +199,20 @@ namespace MultiWheelC
-motionSin * worldVx +
motionCos * worldVy;
// 虚拟阿克曼不能直接执行运动坐标系横向速度，
// 因此将横向纠偏量转换为期望航向修正。
var courseCorrection =
CommandBackend ==
ChassisCommandBackend.VirtualAckermann
? Math.Atan2(
normalCorrection,
Math.Max(
speed,
MinimumSpeed))
: 0.0;
var desiredBodyYaw =
tangentYaw -
tangentYaw +
courseCorrection -
MotionFrameYawInBodyRadians;
var headingError =
FrameTransform2D
@@ -194,31 +226,166 @@ namespace MultiWheelC
omega,
MaximumAngularSpeedRadiansPerSecond);
// 运动坐标系相对车体系旋转+90°：
// 运动系正向速度会转换成车体系+Y速度。
var bodyTwist =
FrameTransform2D
.TransformTwistAtSamePoint(
new Pose2D(
0.0,
0.0,
MotionFrameYawInBodyRadians),
new Twist2D(
vxInMotion,
vyInMotion,
omega));
var now = DateTime.Now;
var interval = now - lastCommandTime;
lastCommandTime = now;
var command = new ChassisCommand(
PilotDefinition.Self.CarNum,
bodyTwist);
if (!adapter.Send(command, interval))
bool commandAccepted;
Twist2D bodyTwist;
if (CommandBackend ==
ChassisCommandBackend.SendMotion)
{
// 运动坐标系相对车体系旋转+90°：
// 运动系正向速度会转换成车体系+Y速度。
bodyTwist =
FrameTransform2D
.TransformTwistAtSamePoint(
new Pose2D(
0.0,
0.0,
MotionFrameYawInBodyRadians),
new Twist2D(
vxInMotion,
vyInMotion,
omega));
// 将运动坐标系原点和前后几何控制点处的速度，
// 转换为SendMotion需要的前后轴方向。
var controlPointRadiusMeters =
Math.Max(
chassis.ControlPointRadius /
1000.0,
0.001);
var frontVelocityY =
vyInMotion +
omega *
controlPointRadiusMeters;
var rearVelocityY =
vyInMotion -
omega *
controlPointRadiusMeters;
var frontSteeringRadians =
Math.Atan2(
frontVelocityY,
vxInMotion);
var rearSteeringRadians =
Math.Atan2(
rearVelocityY,
vxInMotion);
// 蟹行测试绕过M层ManualControl并直接调用SendMotion，
// 因此需要在C层同步应用蟹行虚拟几何比例和转向符号。
if (IsCrabMotionFrame())
{
var geometryRatio =
adapter.HalfTrackWidthMeters /
adapter.HalfWheelBaseMeters;
frontSteeringRadians =
ConvertToCrabSteering(
frontSteeringRadians,
geometryRatio);
rearSteeringRadians =
ConvertToCrabSteering(
rearSteeringRadians,
geometryRatio);
}
var frontThetaDegrees =
(float)(
frontSteeringRadians *
180.0 / Math.PI);
var rearThetaDegrees =
(float)(
rearSteeringRadians *
180.0 / Math.PI);
var motionSpeed =
(float)Math.Sqrt(
vxInMotion * vxInMotion +
vyInMotion * vyInMotion);
commandAccepted =
chassis.SendMotion(
motionSpeed,
frontThetaDegrees,
rearThetaDegrees,
interval);
}
else if (CommandBackend ==
ChassisCommandBackend
.VirtualAckermann)
{
// 虚拟阿克曼以运动坐标系X轴为前向，
// 通过曲率控制转弯，不直接下发横向纠偏速度。
var virtualHalfWheelBaseMeters =
adapter.HalfTrackWidthMeters;
var steeringRadians =
Math.Atan2(
omega *
virtualHalfWheelBaseMeters,
speed);
steeringRadians = Limit(
steeringRadians,
MaximumVirtualSteeringRadians);
// 转向限幅后重新计算实际可下发角速度，
// 保证记录值与底盘最终收到的命令一致。
var acceptedOmega =
speed *
Math.Tan(steeringRadians) /
virtualHalfWheelBaseMeters;
bodyTwist = new Twist2D(
speed *
Math.Cos(
MotionFrameYawInBodyRadians),
speed *
Math.Sin(
MotionFrameYawInBodyRadians),
acceptedOmega);
commandAccepted =
adapter.SendVirtualAckermann(
MotionFrameYawInBodyRadians,
speed,
steeringRadians,
virtualHalfWheelBaseMeters,
interval);
}
else if (CommandBackend ==
ChassisCommandBackend
.SendXYThSpeed)
{
// 安全XYTh后端根据舵角误差统一压低驱动轮速。
bodyTwist =
FrameTransform2D
.TransformTwistAtSamePoint(
new Pose2D(
0.0,
0.0,
MotionFrameYawInBodyRadians),
new Twist2D(
vxInMotion,
vyInMotion,
omega));
var command = new ChassisCommand(
PilotDefinition.Self.CarNum,
bodyTwist);
commandAccepted =
adapter.Send(
command,
interval);
}
else
{
throw new InvalidOperationException(
"蟹行轨迹底盘解算失败：" +
adapter.LastFailureReason);
$"不支持的底盘命令后端：{CommandBackend}。");
}
if (!commandAccepted)
throw new InvalidOperationException(
"运动坐标系轨迹底盘解算失败：" +
chassis
.LastMotionDecomposeFailureReason);
CommandObserver?.Invoke(
(float)bodyTwist.VxMetersPerSecond,
@@ -232,12 +399,46 @@ namespace MultiWheelC
finally
{
adapter.StopImmediately();
if (CommandBackend ==
ChassisCommandBackend.SendMotion)
{
// 测试退出后恢复真实车体坐标系，避免影响后续测试。
adapter.ResetToBodyFrame();
}
CommandObserver?.Invoke(0f, 0f, 0f);
}
yield return false;
}
// 判断当前运动坐标系是否为车体左侧朝前的蟹行坐标系。
private bool IsCrabMotionFrame()
{
return Math.Abs(
FrameTransform2D
.ShortestAngleDifference(
Math.PI / 2.0,
MotionFrameYawInBodyRadians)) <
1e-6;
}
// 按车体几何比例缩小蟹行转角。
// +90°运动坐标系已经完成方向映射，此处不能再次反号。
private double ConvertToCrabSteering(
double normalSteeringRadians,
double geometryRatio)
{
var crabSteeringRadians =
Math.Atan(
geometryRatio *
Math.Tan(
normalSteeringRadians));
return Limit(
crabSteeringRadians,
MaximumVirtualSteeringRadians);
}
// 计算当前点在直线或圆弧上的参考点、切线和剩余距离。
private void CalculateReference(
Vector2 currentPosition,
@@ -642,10 +843,15 @@ namespace MultiWheelC
!IsFinite(SlowDistanceMillimeters) ||
FinishDistanceMillimeters < 0f ||
!IsFinite(FinishDistanceMillimeters) ||
TrackingTimeoutSeconds <= 0f ||
!IsFinite(TrackingTimeoutSeconds))
throw new ArgumentOutOfRangeException(
"蟹行轨迹测试参数无效。");
TrackingTimeoutSeconds <= 0f ||
!IsFinite(TrackingTimeoutSeconds) ||
MaximumVirtualSteeringRadians <= 0.0 ||
MaximumVirtualSteeringRadians >=
Math.PI / 2.0 ||
!IsFinite(
MaximumVirtualSteeringRadians))
throw new ArgumentOutOfRangeException(
"蟹行轨迹测试参数无效。");
}
private static double Limit(
ClumsyPilot/MovementTests.cs
+49 -445
View File
@@ -103,7 +103,7 @@ namespace MultiWheelC
}
}
[MovementTest(name = "旧版SendMotion：连续前进4m")]
[MovementTest(name = "SendMotion：连续前进4m")]
public class TestForward4m : MovementTest
{
public float DistanceMillimeters = 4000f; // 测试距离，单位mm。
@@ -178,15 +178,25 @@ namespace MultiWheelC
}
}
[MovementTest(name = "测试原地自转90°")]
public class TestRotate90 : MovementTest
public abstract class InPlaceRotateTestBase : MovementTest
{
public float RelativeAngleDegrees = 90f; // 相对当前航向的旋转角度，逆时针为正。
public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。
public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。
public int TrialNumber = 1; // 重复实验编号。
private DriveTask _task;
private TrackingExperimentRecorder _recorder;
private readonly string _trajectoryName;
protected InPlaceRotateTestBase(
float relativeAngleDegrees,
string trajectoryName)
{
RelativeAngleDegrees =
relativeAngleDegrees;
_trajectoryName =
trajectoryName;
}
// 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。
public override void Test()
@@ -222,7 +232,7 @@ namespace MultiWheelC
_recorder = new TrackingExperimentRecorder(
controllerName: "InPlaceRotatePID",
trajectoryName: "Rotate90",
trajectoryName: _trajectoryName,
trialNumber: TrialNumber,
referenceStart: rotationCenter,
referenceEnd: rotationCenter,
@@ -297,7 +307,27 @@ namespace MultiWheelC
}
}
[MovementTest(name = "旧版SendMotion：左转90°圆弧")]
[MovementTest(name = "SendXYThSpeed：原地自转90°")]
public sealed class TestRotate90 :
InPlaceRotateTestBase
{
public TestRotate90()
: base(90f, "Rotate90")
{
}
}
[MovementTest(name = "SendXYThSpeed：原地自转180°")]
public sealed class TestRotate180 :
InPlaceRotateTestBase
{
public TestRotate180()
: base(180f, "Rotate180")
{
}
}
[MovementTest(name = "SendMotion：左转90°半径2m圆弧")]
public class TestArcMovement : MovementTest
{
public float RadiusMillimeters = 2000f; // 左转圆的半径，单位mm。
@@ -425,7 +455,7 @@ namespace MultiWheelC
}
}
[MovementTest(name = "测试蟹行前进4m")]
[MovementTest(name = "SendMotion：蟹行直线4m")]
public class TestCrabForward4m : MovementTest
{
public float DistanceMillimeters = 4000f;
@@ -455,6 +485,10 @@ namespace MultiWheelC
var tracker = new CrabMotionFrameTracker
{
CommandBackend =
CrabMotionFrameTracker
.ChassisCommandBackend
.SendMotion,
PathKind =
CrabMotionFrameTracker
.ReferencePathKind.Straight,
@@ -468,7 +502,7 @@ namespace MultiWheelC
_recorder = new TrackingExperimentRecorder(
controllerName:
"CrabMotionFrameTracker",
"CrabSendMotionTracker",
trajectoryName:
"CrabStraight4m",
trialNumber: TrialNumber,
@@ -543,7 +577,7 @@ namespace MultiWheelC
}
}
[MovementTest(name = "测试蟹行左转90°圆弧")]
[MovementTest(name = "SendMotion：蟹行左转90°半径2m圆弧")]
public class TestCrabLeftArc90 : MovementTest
{
public float RadiusMillimeters = 2000f;
@@ -577,6 +611,10 @@ namespace MultiWheelC
location.th * Math.PI / 180.0;
var tracker = new CrabMotionFrameTracker
{
CommandBackend =
CrabMotionFrameTracker
.ChassisCommandBackend
.SendMotion,
PathKind =
CrabMotionFrameTracker
.ReferencePathKind.LeftArc,
@@ -593,7 +631,7 @@ namespace MultiWheelC
_recorder = new TrackingExperimentRecorder(
controllerName:
"CrabMotionFrameTracker",
"CrabSendMotionTracker",
trajectoryName:
$"CrabLeftArc90_R{RadiusMillimeters:0}mm",
trialNumber: TrialNumber,
@@ -639,131 +677,7 @@ namespace MultiWheelC
}
}
[MovementTest(name = "测试蟹行4m S型曲线")]
public class TestCrabSCurve4m : MovementTest
{
public float LengthMillimeters = 4000f;
public float LateralOffsetMillimeters = 400f;
public float CruiseSpeed = 0.2f;
public int TrialNumber = 1;
private DriveTask _task;
private TrackingExperimentRecorder _recorder;
// 将车体左侧作为运动前向，跟踪与普通测试参数一致的4m S型曲线。
public override void Test()
{
if (float.IsNaN(LengthMillimeters) ||
float.IsInfinity(LengthMillimeters) ||
LengthMillimeters <= 0f ||
float.IsNaN(
LateralOffsetMillimeters) ||
float.IsInfinity(
LateralOffsetMillimeters) ||
LateralOffsetMillimeters <= 0f ||
float.IsNaN(CruiseSpeed) ||
float.IsInfinity(CruiseSpeed) ||
CruiseSpeed <= 0f)
{
Console.WriteLine(
"蟹行S型曲线测试参数无效。");
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当前位姿无效，取消蟹行S型曲线测试。");
return;
}
var source = new Vector2(
(float)location.x,
(float)location.y);
var bodyYawRadians =
location.th * Math.PI / 180.0;
var initialMotionYaw =
bodyYawRadians + Math.PI / 2.0;
var destination = new Vector2(
source.X +
LengthMillimeters *
(float)Math.Cos(
initialMotionYaw),
source.Y +
LengthMillimeters *
(float)Math.Sin(
initialMotionYaw));
var tracker = new CrabMotionFrameTracker
{
PathKind =
CrabMotionFrameTracker
.ReferencePathKind.SCurve,
StartPosition = source,
InitialBodyYawRadians =
bodyYawRadians,
LengthMillimeters =
LengthMillimeters,
SCurveLateralOffsetMillimeters =
LateralOffsetMillimeters,
CruiseSpeed = CruiseSpeed
};
_recorder = new TrackingExperimentRecorder(
controllerName:
"CrabMotionFrameTracker",
trajectoryName:
$"CrabSCurve4m_A{LateralOffsetMillimeters:0}mm",
trialNumber: TrialNumber,
referenceStart: source,
referenceEnd: destination,
referenceSpeed: CruiseSpeed,
referenceMotionFrameYawDegrees: 90f);
tracker.CommandObserver =
(vx, vy, omega) =>
_recorder?.UpdateBodyCommand(
vx,
vy,
omega);
_recorder.Start();
try
{
_task = new DriveTask(tracker.Get());
_task.Wait();
Thread.Sleep(300);
}
finally
{
_task?.Stop();
_recorder?.UpdateBodyCommand(
0f,
0f,
0f);
_recorder?.StopAndSave();
_task = null;
_recorder = null;
}
}
public override void TestStop()
{
_task?.Stop();
_recorder?.UpdateBodyCommand(
0f,
0f,
0f);
_recorder?.StopAndSave();
}
}
[MovementTest(name = "旧版SendMotion：4m S型曲线")]
[MovementTest(name = "SendMotion：4m S型曲线")]
public class TestSCurve4m : MovementTest
{
public float LengthMillimeters = 4000f; // S型曲线纵向长度，单位mm。
@@ -955,193 +869,6 @@ namespace MultiWheelC
}
}
// C层单车测试：统一使用车体速度命令和SendXYThSpeed跟踪普通模式轨迹。
public abstract class XYThNormalTrajectoryTestBase : MovementTest
{
public float LengthMillimeters = 4000f;
public float RadiusMillimeters = 2000f;
public float LateralOffsetMillimeters = 400f;
public float CruiseSpeed = 0.3f;
public int TrialNumber = 1;
private DriveTask _task;
private TrackingExperimentRecorder _recorder;
protected abstract CrabMotionFrameTracker.ReferencePathKind ReferencePath { get; }
protected abstract string TrajectoryName { get; }
// C层单车测试：读取Detour起点并执行普通模式SendXYThSpeed轨迹。
public override void Test()
{
ValidateParameters();
if (!TryReadDetourPose(
out var source,
out var initialBodyYawRadians))
{
throw new InvalidOperationException(
"Detour当前位置或航向无效，无法开始新版SendXYThSpeed测试。");
}
var tracker = new CrabMotionFrameTracker
{
PathKind = ReferencePath,
// 普通模式的运动坐标系与车体坐标系重合。
MotionFrameYawInBodyRadians = 0.0,
StartPosition = source,
InitialBodyYawRadians = initialBodyYawRadians,
LengthMillimeters = LengthMillimeters,
RadiusMillimeters = RadiusMillimeters,
ArcSweepRadians = Math.PI / 2.0,
SCurveLateralOffsetMillimeters =
LateralOffsetMillimeters,
CruiseSpeed = CruiseSpeed,
};
var destination = ReferencePath ==
CrabMotionFrameTracker.ReferencePathKind
.LeftArc
? tracker.GetArcDestination()
: new Vector2(
source.X +
LengthMillimeters *
(float)Math.Cos(initialBodyYawRadians),
source.Y +
LengthMillimeters *
(float)Math.Sin(initialBodyYawRadians));
_recorder = new TrackingExperimentRecorder(
controllerName: "UnifiedXYThTracker",
trajectoryName: TrajectoryName,
trialNumber: TrialNumber,
referenceStart: source,
referenceEnd: destination,
referenceSpeed: CruiseSpeed);
tracker.CommandObserver =
(vx, vy, omegaRadiansPerSecond) =>
_recorder?.UpdateBodyCommand(
vx,
vy,
omegaRadiansPerSecond);
_recorder.Start();
_task = new DriveTask(tracker.Get());
try
{
_task.Wait();
}
finally
{
_recorder?.UpdateBodyCommand(0f, 0f, 0f);
_recorder?.StopAndSave();
_recorder = null;
_task = null;
}
}
// C层单车测试：停止新版SendXYThSpeed轨迹并保存已有记录。
public override void TestStop()
{
_task?.Stop();
_recorder?.UpdateBodyCommand(0f, 0f, 0f);
_recorder?.StopAndSave();
}
// C层单车测试：检查新版轨迹的长度、半径、偏移和速度参数。
private void ValidateParameters()
{
if (!IsPositiveFinite(LengthMillimeters))
throw new ArgumentOutOfRangeException(
nameof(LengthMillimeters),
"轨迹长度必须是正有限值。");
if (!IsPositiveFinite(RadiusMillimeters))
throw new ArgumentOutOfRangeException(
nameof(RadiusMillimeters),
"圆弧半径必须是正有限值。");
if (!IsPositiveFinite(LateralOffsetMillimeters))
throw new ArgumentOutOfRangeException(
nameof(LateralOffsetMillimeters),
"S型曲线横向偏移必须是正有限值。");
if (!IsPositiveFinite(CruiseSpeed))
throw new ArgumentOutOfRangeException(
nameof(CruiseSpeed),
"巡航速度必须是正有限值。");
}
// C层单车测试：读取并验证Detour毫米坐标和角度制航向。
private static bool TryReadDetourPose(
out Vector2 position,
out double yawRadians)
{
var location = DetourInterface.getCartLocation();
var x = location.x;
var y = location.y;
var thetaDegrees = location.th;
position = new Vector2((float)x, (float)y);
yawRadians = thetaDegrees * Math.PI / 180.0;
return IsFinite(x) &&
IsFinite(y) &&
IsFinite(thetaDegrees);
}
// C层单车测试：判断浮点参数是否为有限值。
private static bool IsFinite(double value)
{
return !double.IsNaN(value) &&
!double.IsInfinity(value);
}
// C层单车测试：判断浮点参数是否为正有限值。
private static bool IsPositiveFinite(double value)
{
return IsFinite(value) && value > 0.0;
}
}
[MovementTest(name = "新版SendXYThSpeed：连续前进4m")]
public sealed class TestXYThForward4m :
XYThNormalTrajectoryTestBase
{
protected override CrabMotionFrameTracker.ReferencePathKind
ReferencePath =>
CrabMotionFrameTracker.ReferencePathKind.Straight;
protected override string TrajectoryName =>
"XYThStraight4m";
}
[MovementTest(name = "新版SendXYThSpeed：左转90°圆弧")]
public sealed class TestXYThLeftArc90 :
XYThNormalTrajectoryTestBase
{
protected override CrabMotionFrameTracker.ReferencePathKind
ReferencePath =>
CrabMotionFrameTracker.ReferencePathKind.LeftArc;
protected override string TrajectoryName =>
$"XYThLeftArc90_R{RadiusMillimeters:0}mm";
}
[MovementTest(name = "新版SendXYThSpeed：4m S型曲线")]
public sealed class TestXYThSCurve4m :
XYThNormalTrajectoryTestBase
{
protected override CrabMotionFrameTracker.ReferencePathKind
ReferencePath =>
CrabMotionFrameTracker.ReferencePathKind.SCurve;
protected override string TrajectoryName =>
$"XYThSCurve4m_A{LateralOffsetMillimeters:0}mm";
}
public abstract class ClampMovementTestBase : MovementTest
{
public float TimeoutSeconds = 30f; // 动作超时时间，单位s。
@@ -1223,126 +950,3 @@ namespace MultiWheelC
}
#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
+71 -5
View File
@@ -436,33 +436,99 @@ namespace MultiWheelC
// 将本周期PID角速度输出提供给实验记录器，单位deg/s。
public Action<float> CommandAngularSpeedObserver;
// 自转前舵轮实际角度允许误差，单位deg。
public float WheelAlignmentToleranceDegrees = 2f;
// 自转舵轮连续保持到位的时间，单位s。
public float WheelAlignmentStableSeconds = 0.3f;
// 自转舵轮准备超时时间，单位s。
public float WheelAlignmentTimeoutSeconds = 10f;
// 归一化到大约 [-180°, 180°]
private static float RangeAngle(float theta)
{
return (float)(theta - Math.Round(theta / 360.0f) * 360);
}
// 使用 PID 控制原地旋转到目标角度。
// 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。
public override IEnumerable<bool> Get()
{
if (Chassis == null)
throw new InvalidOperationException(
"当前底盘不是MultiWheelChassis，无法执行原地自转。");
var adapter = new MultiWheelChassisAdapter(
Chassis,
PilotDefinition.Self.CarNum);
try
{
var alignmentStarted = DateTime.Now;
DateTime? alignedSince = null;
while (true)
{
if (!adapter.PrepareSpin())
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 targetAngle = RangeAngle(AngleTarget);
var p = PidparamsRead();
thPid = new PIDController(ThetaReader, p.Kp);
thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
p.OutputUpperThreshold, p.SpeedAccPerSec);
var lastCommandTime = DateTime.Now;
while (true)
{
var s = thPid.GetResponse(targetAngle, true);
Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
CommandAngularSpeedObserver?.Invoke(s);
if (!Chassis.SendRotateMotion(s))
var now = DateTime.Now;
var interval = now - lastCommandTime;
lastCommandTime = now;
// PID输出s为deg/s，Shared命令统一使用rad/s。
// adapter.Send最终调用普通安全版SendXYThSpeed。
var omegaRadiansPerSecond =
s * (float)Math.PI / 180f;
if (!adapter.Send(
new ChassisCommand(
PilotDefinition.Self.CarNum,
new Twist2D(
0.0,
0.0,
omegaRadiansPerSecond)),
interval))
{
throw new InvalidOperationException(
"原地旋转底盘解算失败：" +
Chassis.LastMotionDecomposeFailureReason);
"安全XYTh原地旋转底盘解算失败：" +
adapter.LastFailureReason);
}
if (thPid.IsArrived()) break;
yield return true;
@@ -473,7 +539,7 @@ namespace MultiWheelC
finally
{
CommandAngularSpeedObserver?.Invoke(0f);
Chassis.PredefinedDriveStop();
adapter.StopImmediately();
}
}
}
ClumsyPilot/build/Clumsy/ClumsyPilot.dll
BIN
View File
Binary file not shown.
ClumsyPilot/build/Clumsy/ClumsyPilot.pdb
BIN
View File
Binary file not shown.
ClumsyPilot/build/Clumsy/CommonUsage.dll
BIN
View File
Binary file not shown.
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
@@ -14,7 +14,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")]
[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0+7e05ff098e34d47e777a6809a99952fdb24bf5a2")]
[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 @@
5d77794fa0720c6591db5b06ac60427413c18989a6f7b64420ccb07d122d85bc
ad10c8964b21ff4923ef175481b5fc5b35f6275f95224074d0c26f781df45851
ClumsyPilot/obj/Debug/ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig
+1 -1
View File
@@ -1,6 +1,6 @@
is_global = true
build_property.RootNamespace = MultiWheelC
build_property.ProjectDir = D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\
build_property.ProjectDir = D:\MyParking\ClumsyPilot\
build_property.EnableComHosting =
build_property.EnableGeneratedComInterfaceComImportInterop =
build_property.CsWinRTUseWindowsUIXamlProjections = false
ClumsyPilot/obj/Debug/ClumsyPilot.assets.cache
BIN
View File
Binary file not shown.
ClumsyPilot/obj/Debug/ClumsyPilot.csproj.AssemblyReference.cache
BIN
View File
Binary file not shown.
ClumsyPilot/obj/Debug/ClumsyPilot.dll
BIN
View File
Binary file not shown.
ClumsyPilot/obj/Debug/ClumsyPilot.pdb
BIN
View File
Binary file not shown.
ClumsyPilot/ref/CommonUsage.dll
BIN
View File
Binary file not shown.