diff --git a/ClumsyPilot/CrabMotionFrameTracker.cs b/ClumsyPilot/CrabMotionFrameTracker.cs index be4408b..5297abd 100644 --- a/ClumsyPilot/CrabMotionFrameTracker.cs +++ b/ClumsyPilot/CrabMotionFrameTracker.cs @@ -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( diff --git a/ClumsyPilot/MovementTests.cs b/ClumsyPilot/MovementTests.cs index ed8e81b..b1e373d 100644 --- a/ClumsyPilot/MovementTests.cs +++ b/ClumsyPilot/MovementTests.cs @@ -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 diff --git a/ClumsyPilot/Movements.cs b/ClumsyPilot/Movements.cs index 8423cea..6d35654 100644 --- a/ClumsyPilot/Movements.cs +++ b/ClumsyPilot/Movements.cs @@ -436,33 +436,99 @@ namespace MultiWheelC // 将本周期PID角速度输出提供给实验记录器，单位deg/s。 public Action 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 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(); } } } diff --git a/ClumsyPilot/build/Clumsy/ClumsyPilot.dll b/ClumsyPilot/build/Clumsy/ClumsyPilot.dll deleted file mode 100644 index 754de48..0000000 Binary files a/ClumsyPilot/build/Clumsy/ClumsyPilot.dll and /dev/null differ diff --git a/ClumsyPilot/build/Clumsy/ClumsyPilot.pdb b/ClumsyPilot/build/Clumsy/ClumsyPilot.pdb deleted file mode 100644 index 7dbbf74..0000000 Binary files a/ClumsyPilot/build/Clumsy/ClumsyPilot.pdb and /dev/null differ diff --git a/ClumsyPilot/build/Clumsy/CommonUsage.dll b/ClumsyPilot/build/Clumsy/CommonUsage.dll index 209c020..37c2382 100644 Binary files a/ClumsyPilot/build/Clumsy/CommonUsage.dll and b/ClumsyPilot/build/Clumsy/CommonUsage.dll differ diff --git a/ClumsyPilot/build/Clumsy/MultiWheelC.dll b/ClumsyPilot/build/Clumsy/MultiWheelC.dll index 8837123..6a1eac5 100644 Binary files a/ClumsyPilot/build/Clumsy/MultiWheelC.dll and b/ClumsyPilot/build/Clumsy/MultiWheelC.dll differ diff --git a/ClumsyPilot/build/Clumsy/MultiWheelC.pdb b/ClumsyPilot/build/Clumsy/MultiWheelC.pdb index ab65ced..7eb8ed0 100644 Binary files a/ClumsyPilot/build/Clumsy/MultiWheelC.pdb and b/ClumsyPilot/build/Clumsy/MultiWheelC.pdb differ diff --git a/ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfo.cs b/ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfo.cs index 4f4d0a5..5160759 100644 --- a/ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfo.cs +++ b/ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfo.cs @@ -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")] diff --git a/ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfoInputs.cache b/ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfoInputs.cache index 8fb9fb2..1e9a110 100644 --- a/ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfoInputs.cache +++ b/ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfoInputs.cache @@ -1 +1 @@ -5d77794fa0720c6591db5b06ac60427413c18989a6f7b64420ccb07d122d85bc +ad10c8964b21ff4923ef175481b5fc5b35f6275f95224074d0c26f781df45851 diff --git a/ClumsyPilot/obj/Debug/ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig b/ClumsyPilot/obj/Debug/ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig index a3aa451..8ce14cc 100644 --- a/ClumsyPilot/obj/Debug/ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig +++ b/ClumsyPilot/obj/Debug/ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig @@ -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 diff --git a/ClumsyPilot/obj/Debug/ClumsyPilot.assets.cache b/ClumsyPilot/obj/Debug/ClumsyPilot.assets.cache index 3d28e6f..2afc373 100644 Binary files a/ClumsyPilot/obj/Debug/ClumsyPilot.assets.cache and b/ClumsyPilot/obj/Debug/ClumsyPilot.assets.cache differ diff --git a/ClumsyPilot/obj/Debug/ClumsyPilot.csproj.AssemblyReference.cache b/ClumsyPilot/obj/Debug/ClumsyPilot.csproj.AssemblyReference.cache index 1ce4166..95684d2 100644 Binary files a/ClumsyPilot/obj/Debug/ClumsyPilot.csproj.AssemblyReference.cache and b/ClumsyPilot/obj/Debug/ClumsyPilot.csproj.AssemblyReference.cache differ diff --git a/ClumsyPilot/obj/Debug/ClumsyPilot.dll b/ClumsyPilot/obj/Debug/ClumsyPilot.dll index 754de48..6a1eac5 100644 Binary files a/ClumsyPilot/obj/Debug/ClumsyPilot.dll and b/ClumsyPilot/obj/Debug/ClumsyPilot.dll differ diff --git a/ClumsyPilot/obj/Debug/ClumsyPilot.pdb b/ClumsyPilot/obj/Debug/ClumsyPilot.pdb index 7dbbf74..7eb8ed0 100644 Binary files a/ClumsyPilot/obj/Debug/ClumsyPilot.pdb and b/ClumsyPilot/obj/Debug/ClumsyPilot.pdb differ diff --git a/ClumsyPilot/ref/CommonUsage.dll b/ClumsyPilot/ref/CommonUsage.dll deleted file mode 100644 index effb81e..0000000 Binary files a/ClumsyPilot/ref/CommonUsage.dll and /dev/null differ diff --git a/CommonUsage-MultiVehicleSync/commonusage/Chassis/MultiWheelChassis.cs b/CommonUsage-MultiVehicleSync/commonusage/Chassis/MultiWheelChassis.cs index 110b667..1576213 100644 --- a/CommonUsage-MultiVehicleSync/commonusage/Chassis/MultiWheelChassis.cs +++ b/CommonUsage-MultiVehicleSync/commonusage/Chassis/MultiWheelChassis.cs @@ -10,19 +10,19 @@ using System.Drawing; namespace CommonUsage.Chassis { - public class MultiWheelChassis : AbstractChassis - { - public float ThConsistentThreshold = 1.0f; + public class MultiWheelChassis : AbstractChassis + { + public float ThConsistentThreshold = 1.0f; - public float ControlPointRadius = 500f; + public float ControlPointRadius = 500f; - // 原地旋转纠偏钳位：SendRotateMotion 中每轮纠偏速度幅值不超过 该比例×本轮旋转切向速度， - // 防止减速末段旋转切向变小时纠偏盖过它、使轮向矢量乱摆（频繁打方向/卡死）。<0 关闭钳位。 - public float RotateCompTangentFrac = 0.5f; + // 原地旋转纠偏钳位：SendRotateMotion 中每轮纠偏速度幅值不超过 该比例×本轮旋转切向速度， + // 防止减速末段旋转切向变小时纠偏盖过它、使轮向矢量乱摆（频繁打方向/卡死）。<0 关闭钳位。 + public float RotateCompTangentFrac = 0.5f; - // 上一次 SendRotateMotion 的舵轮对齐状态：false 表示舵轮未追上目标角(gate=0、车未真正转动)。 - // 供上层做积分抗饱和（卡死时冻结积分）。 - public bool LastRotateAligned { get; private set; } = true; + // 上一次 SendRotateMotion 的舵轮对齐状态：false 表示舵轮未追上目标角(gate=0、车未真正转动)。 + // 供上层做积分抗饱和（卡死时冻结积分）。 + public bool LastRotateAligned { get; private set; } = true; public string LastMotionDecomposeFailureReason { get; private set; } = ""; @@ -33,34 +33,34 @@ namespace CommonUsage.Chassis public float MinimumTurningAngleForAckermann = 60f; - public MultiWheelChassis() : base() - { + public MultiWheelChassis() : base() + { - } + } - public void AddWheel(SteerWheel wheel) - { - _steerWheels.Add(wheel); - } + public void AddWheel(SteerWheel wheel) + { + _steerWheels.Add(wheel); + } - public override void Initialize() - { - _steerWheels = _steerWheels.OrderByDescending(sw => sw.Position.X).ToList(); - _targetSpeeds = new float[_steerWheels.Count].ToList(); + public override void Initialize() + { + _steerWheels = _steerWheels.OrderByDescending(sw => sw.Position.X).ToList(); + _targetSpeeds = new float[_steerWheels.Count].ToList(); _sendSpeeds = new float[_steerWheels.Count].ToList(); _tmpSpeeds = new float[_steerWheels.Count].ToList(); _wheelDirs = Enumerable.Repeat(1, _steerWheels.Count).ToList(); - _sendAngle = new float[_steerWheels.Count].ToList(); + _sendAngle = new float[_steerWheels.Count].ToList(); _debugSpeeds = new float[_steerWheels.Count].ToList(); if (_steerWheels.Count < 2) - { - Console.WriteLine($"steer wheel num: {_steerWheels.Count}. invalid!"); - Valid = false; - return; - } + { + Console.WriteLine($"steer wheel num: {_steerWheels.Count}. invalid!"); + Valid = false; + return; + } - CalculateAxes(); + CalculateAxes(); Valid = true; } @@ -109,7 +109,7 @@ namespace CommonUsage.Chassis } public override void AfterDirectionChanged() - { + { Hedingben.ToastText($"AfterDirectionChanged", "MultiWheelChassis-AfterDirectionChanged"); foreach (var sw in _steerWheels) @@ -162,26 +162,26 @@ namespace CommonUsage.Chassis private float _lastGcpTheta1; - /// - /// 转速单位为度/s，逆时针为正。 - /// - /// - public override bool ComputeRotateWheels(float rotSpeed) - { - if (!RotatingActive) ResetMotionState(); + /// + /// 转速单位为度/s，逆时针为正。 + /// + /// + public override bool ComputeRotateWheels(float rotSpeed) + { + if (!RotatingActive) ResetMotionState(); - var feasible = SendRotateMotion(rotSpeed); + var feasible = SendRotateMotion(rotSpeed); - GoingActive = false; - RotatingActive = true; - XYThActive = false; - return feasible; - } + GoingActive = false; + RotatingActive = true; + XYThActive = false; + return feasible; + } - public override void PredefinedDriveStop() - { + public override void PredefinedDriveStop() + { if (!Valid) return; - for (var i = 0; i < _steerWheels.Count; i++) + for (var i = 0; i < _steerWheels.Count; i++) { _sendSpeeds[i] = 0; _debugSpeeds[i] = 0; @@ -195,10 +195,10 @@ namespace CommonUsage.Chassis _steerWheels[i].WriteSpeed(0); } } - GoingActive = false; - RotatingActive = false; - XYThActive = false; - } + GoingActive = false; + RotatingActive = false; + XYThActive = false; + } public void RampStop(TimeSpan? deltaTime = null) { @@ -216,6 +216,48 @@ namespace CommonUsage.Chassis LastMoveTime = DateTime.Now; } + /// + /// 立即清零XYTh驱动轮速度，同时保留已经准备好的舵角目标和轮速方向。 + /// 下一条非零命令仍需重新确认四轮实际舵角到位后才会开放驱动速度。 + /// + public void StopXYThDrivePreserveSteeringState() + { + if (!Valid) return; + + // 只有已完成Prepare/Adopt交接的XYTh模式才能保留状态。 + if (!XYThActive) + { + PredefinedDriveStop(); + return; + } + + for (var i = 0; i < _steerWheels.Count; i++) + { + _targetSpeeds[i] = 0; + _sendSpeeds[i] = 0; + _debugSpeeds[i] = 0; + + if (_steerWheels[i] is DiffSteerWheel diffSteerWheel) + { + diffSteerWheel.WriteLeftSpeed(0); + diffSteerWheel.WriteRightSpeed(0); + } + else + { + _steerWheels[i].WriteSpeed(0); + } + } + + GoingActive = false; + RotatingActive = false; + + // 保留XYThActive、_sendAngle和_wheelDirs，避免重新选择等价舵角； + // 清除到位标记，使下一次推动摇杆时重新核对实际反馈。 + _xyThWheelsAligned = false; + LastMoveTime = DateTime.Now; + LastMotionDecomposeFailureReason = ""; + } + private struct WheelAngleCandidate { public bool Valid; @@ -393,8 +435,8 @@ namespace CommonUsage.Chassis /// public bool SendMotion(float speed, float frontTh, float rearTh, TimeSpan? deltaTime = null, float localControlRadius = 0, float localCompensateX = 0f, float localCompensateY = 0, float localCompensateTh = 0) - { - if (!Valid) return FailMotionDecomposition("SendMotion", "invalid chassis", deltaTime); + { + if (!Valid) return FailMotionDecomposition("SendMotion", "invalid chassis", deltaTime); LastMotionDecomposeFailureReason = ""; var compInputSpeed = speed; @@ -463,11 +505,11 @@ namespace CommonUsage.Chassis } float Interpolate(float left, float current, float right) - { - if (current <= left) return 0; - if (current >= right) return 1; - return (current - left) / (right - left); - } + { + if (current <= left) return 0; + if (current >= right) return 1; + return (current - left) / (right - left); + } while (needAdjust && scaleFactor > minScaleFactor) { @@ -477,7 +519,7 @@ namespace CommonUsage.Chassis // ... 现有的插值函数和旋转中心计算代码 ... var thDiff = CommonMath.ThDiff(currentFrontTh, currentRearTh); axisDiffFlag = Math.Abs(thDiff) > ThConsistentThreshold; - + var sign = -1; if (axisDiffFlag) { @@ -641,14 +683,14 @@ namespace CommonUsage.Chassis } var speedBeforeAlignGate = speed; - if (!GoingWheelAligned) - { + if (!GoingWheelAligned) + { Hedingben.ToastText("Wheel Not Aligned", "MultiWheelChassis-SendMotion-notAligned"); - speed = 0; - var aligned = _steerWheels.Select((sw, i) => (sw, i)).All(ww => - Math.Abs(CommonMath.ThDiff(ww.sw.ReadAngle(), _sendAngle[ww.i])) < 1); - GoingWheelAligned = aligned; - } + speed = 0; + var aligned = _steerWheels.Select((sw, i) => (sw, i)).All(ww => + Math.Abs(CommonMath.ThDiff(ww.sw.ReadAngle(), _sendAngle[ww.i])) < 1); + GoingWheelAligned = aligned; + } for (var i = 0; i < _steerWheels.Count; i++) { @@ -693,99 +735,168 @@ namespace CommonUsage.Chassis return true; } - /// - /// 停车并将四个舵轮转到绕当前坐标原点自转所需的切线方向。 - /// 只下发舵角，不下发驱动速度。 - /// - public bool PrepareRotateWheels(float alignmentToleranceDegrees = 2.0f) - { - if (!Valid) - return FailMotionDecomposition( - "PrepareRotateWheels", - "invalid chassis", - null); + /// + /// 停车并将四个舵轮转到绕当前坐标原点自转所需的切线方向。 + /// 只下发舵角，不下发驱动速度。 + /// + public bool PrepareRotateWheels(float alignmentToleranceDegrees = 2.0f) + { + if (!Valid) + return FailMotionDecomposition( + "PrepareRotateWheels", + "invalid chassis", + null); - if (float.IsNaN(alignmentToleranceDegrees) || - float.IsInfinity(alignmentToleranceDegrees) || - alignmentToleranceDegrees < 0.0f) - throw new ArgumentOutOfRangeException( - nameof(alignmentToleranceDegrees), - "自转舵轮到位容差必须是非负有限值。"); + if (float.IsNaN(alignmentToleranceDegrees) || + float.IsInfinity(alignmentToleranceDegrees) || + alignmentToleranceDegrees < 0.0f) + throw new ArgumentOutOfRangeException( + nameof(alignmentToleranceDegrees), + "自转舵轮到位容差必须是非负有限值。"); - if (_steerWheels.Count == 0) - return FailMotionDecomposition( - "PrepareRotateWheels", - "no steer wheels", - null); + if (_steerWheels.Count == 0) + return FailMotionDecomposition( + "PrepareRotateWheels", + "no steer wheels", + null); - // 模式切换期间必须保持驱动轮停止。 - PredefinedDriveStop(); + // 模式切换期间必须保持驱动轮停止。 + PredefinedDriveStop(); - var targetAngles = new float[_steerWheels.Count]; - var directions = new int[_steerWheels.Count]; + var targetAngles = new float[_steerWheels.Count]; + var directions = new int[_steerWheels.Count]; - // 先完成全部舵角解算，再统一下发，避免只转动部分舵轮。 - for (var i = 0; i < _steerWheels.Count; i++) - { - var wheel = _steerWheels[i]; - var px = (double)wheel.Position.X; - var py = (double)wheel.Position.Y; + // 先完成全部舵角解算，再统一下发，避免只转动部分舵轮。 + for (var i = 0; i < _steerWheels.Count; i++) + { + var wheel = _steerWheels[i]; + var px = (double)wheel.Position.X; + var py = (double)wheel.Position.Y; - // 逆时针绕原点旋转时，该舵轮的切向方向为(-py, px)。 - var tangentDegrees = - (float)(Math.Atan2(px, -py) / - Math.PI * 180.0); - tangentDegrees = CommonMath.ThDiff( - tangentDegrees, - wheel.ZeroDirection); + // 逆时针绕原点旋转时，该舵轮的切向方向为(-py, px)。 + var tangentDegrees = + (float)(Math.Atan2(px, -py) / + Math.PI * 180.0); + tangentDegrees = CommonMath.ThDiff( + tangentDegrees, + wheel.ZeroDirection); - if (!TryResolveWheelAngle( - i, - tangentDegrees, - "PrepareRotateWheels", - out targetAngles[i], - out directions[i], - out var reason)) - return FailMotionDecomposition( - "PrepareRotateWheels", - reason, - null); - } + if (!TryResolveWheelAngle( + i, + tangentDegrees, + "PrepareRotateWheels", + out targetAngles[i], + out directions[i], + out var reason)) + return FailMotionDecomposition( + "PrepareRotateWheels", + reason, + null); + } - for (var i = 0; i < _steerWheels.Count; i++) - { - _wheelDirs[i] = directions[i]; - SendTh(i, targetAngles[i]); - } + for (var i = 0; i < _steerWheels.Count; i++) + { + _wheelDirs[i] = directions[i]; + SendTh(i, targetAngles[i]); + } - var allAligned = true; - for (var i = 0; i < _steerWheels.Count; i++) - { - var actualAngle = _steerWheels[i].ReadAngle(); - var angleError = targetAngles[i] - actualAngle; + var allAligned = true; + for (var i = 0; i < _steerWheels.Count; i++) + { + var actualAngle = _steerWheels[i].ReadAngle(); + var angleError = targetAngles[i] - actualAngle; - // 这里比较受机械限位约束的真实舵角，不能使用圆周最短角度差。 - if (float.IsNaN(actualAngle) || - float.IsInfinity(actualAngle) || - Math.Abs(angleError) > alignmentToleranceDegrees) - allAligned = false; - } + // 这里比较受机械限位约束的真实舵角，不能使用圆周最短角度差。 + if (float.IsNaN(actualAngle) || + float.IsInfinity(actualAngle) || + Math.Abs(angleError) > alignmentToleranceDegrees) + allAligned = false; + } - LastRotateAligned = allAligned; - LastMotionDecomposeFailureReason = ""; - return true; - } + LastRotateAligned = allAligned; + LastMotionDecomposeFailureReason = ""; + return true; + } - /// - /// 绕"已被 SetOriginBias 偏置到车队中心的原点"做原地旋转，可叠加一个车体系小幅纠偏旋量。 - /// - /// 绕车队中心角速度(deg/s，逆时针为正)。 - /// 车体系纵向(前+)修正速度(mm/s)，多车联动维持队形用。 - /// 车体系横向(左+)修正速度(mm/s)。 - /// 绕本车几何中心附加角速度(deg/s)，修正朝向偏差。 - public bool SendRotateMotion(float rotSpeed, TimeSpan? deltaTime = null, - float localCompensateX = 0f, float localCompensateY = 0f, float localCompensateTh = 0f) - { + /// + /// 将PrepareRotateWheels已经确认到位的舵角和轮速方向， + /// 原样交接给SendXYThSpeed，作为一段XYTh运动的初始状态。 + /// 该方法不会调用ResetMotionState，因此不会重新选择等价舵角。 + /// + public bool AdoptPreparedRotateWheelsForXYTh( + float alignmentToleranceDegrees = 2.0f) + { + if (!Valid) + return FailMotionDecomposition( + "AdoptPreparedRotateWheelsForXYTh", + "invalid chassis", + null); + + if (float.IsNaN(alignmentToleranceDegrees) || + float.IsInfinity(alignmentToleranceDegrees) || + alignmentToleranceDegrees < 0.0f) + throw new ArgumentOutOfRangeException( + nameof(alignmentToleranceDegrees), + "自转舵轮交接容差必须是非负有限值。"); + + if (!LastRotateAligned) + return FailMotionDecomposition( + "AdoptPreparedRotateWheelsForXYTh", + "rotate wheels have not been prepared and aligned", + null); + + for (var i = 0; i < _steerWheels.Count; i++) + { + var actualAngle = + _steerWheels[i].ReadAngle(); + + if (float.IsNaN(actualAngle) || + float.IsInfinity(actualAngle)) + { + LastRotateAligned = false; + return FailMotionDecomposition( + "AdoptPreparedRotateWheelsForXYTh", + $"wheel {i} angle feedback is invalid: {actualAngle}", + null); + } + + // 比较受机械限位约束的实际舵角，不使用圆周最短角。 + var angleError = + _sendAngle[i] - actualAngle; + if (Math.Abs(angleError) > + alignmentToleranceDegrees) + { + LastRotateAligned = false; + return FailMotionDecomposition( + "AdoptPreparedRotateWheelsForXYTh", + $"wheel {i} is no longer aligned: " + + $"target={_sendAngle[i]:F1}, actual={actualAngle:F1}, " + + $"error={angleError:F1}", + null); + } + } + + // 直接继承PrepareRotateWheels写入的_wheelDirs和_sendAngle。 + // 下一次SendXYThSpeed调用看到XYThActive=true时不会重置这些状态。 + XYThActive = true; + _xyThWheelsAligned = true; + GoingActive = false; + RotatingActive = false; + LastMoveTime = DateTime.Now; + LastMotionDecomposeFailureReason = ""; + return true; + } + + /// + /// 绕"已被 SetOriginBias 偏置到车队中心的原点"做原地旋转，可叠加一个车体系小幅纠偏旋量。 + /// + /// 绕车队中心角速度(deg/s，逆时针为正)。 + /// 车体系纵向(前+)修正速度(mm/s)，多车联动维持队形用。 + /// 车体系横向(左+)修正速度(mm/s)。 + /// 绕本车几何中心附加角速度(deg/s)，修正朝向偏差。 + public bool SendRotateMotion(float rotSpeed, TimeSpan? deltaTime = null, + float localCompensateX = 0f, float localCompensateY = 0f, float localCompensateTh = 0f) + { if (!Valid) return FailMotionDecomposition("SendRotateMotion", "invalid chassis", deltaTime); LastMotionDecomposeFailureReason = ""; if (Math.Abs(rotSpeed) < 1e-6f && @@ -798,10 +909,10 @@ namespace CommonUsage.Chassis return true; } - var ths = new float[_steerWheels.Count]; - var dirs = new int[_steerWheels.Count]; - // 每轮合速度大小(m/s)，含"绕队心旋转 + 车体平移纠偏 + 绕本车中心微转纠偏"三项矢量和。 - var speedMags = new float[_steerWheels.Count]; + var ths = new float[_steerWheels.Count]; + var dirs = new int[_steerWheels.Count]; + // 每轮合速度大小(m/s)，含"绕队心旋转 + 车体平移纠偏 + 绕本车中心微转纠偏"三项矢量和。 + var speedMags = new float[_steerWheels.Count]; var allWheelAligned = true; // 把车体系纠偏旋量换算到 sw.Position 所在的偏置帧 F(原点=车队中心, 朝向随 _originBiasTh)。 @@ -816,9 +927,9 @@ namespace CommonUsage.Chassis var biasX = (double)_originBiasX; // 本车几何中心在 F 中的位置 var biasY = (double)_originBiasY; - for (var i = 0; i < _steerWheels.Count; i++) - { - var sw = _steerWheels[i]; + for (var i = 0; i < _steerWheels.Count; i++) + { + var sw = _steerWheels[i]; var px = (double)sw.Position.X; var py = (double)sw.Position.Y; // 合成轮速矢量(mm/s, F帧)：v = ω_rot ẑ×p + [R(bias)·v_comp + ω_comp ẑ×(p - 本车中心)] @@ -856,10 +967,10 @@ namespace CommonUsage.Chassis out var resolveReason)) return FailMotionDecomposition("SendRotateMotion", resolveReason, deltaTime); _wheelDirs[i] = dirs[i]; - } + } - for (var i = 0; i < _steerWheels.Count; ++i) - SendTh(i, ths[i]); + for (var i = 0; i < _steerWheels.Count; ++i) + SendTh(i, ths[i]); var slowFac = 1f; var maxDth = 0f; for (var i = 0; i < _steerWheels.Count; ++i) @@ -907,16 +1018,16 @@ namespace CommonUsage.Chassis private DateTime _rotDbgLast = DateTime.MinValue; - public override float CalculateTurningSpeedDecayFac(float turn) - { - return 1 - Math.Min(turn, MaxTurnThreshold) / MaxTurnThreshold * MinTurnSpeedFac; - } + public override float CalculateTurningSpeedDecayFac(float turn) + { + return 1 - Math.Min(turn, MaxTurnThreshold) / MaxTurnThreshold * MinTurnSpeedFac; + } [Obsolete] - public List GetSteerWheels() - { - return _steerWheels.ToList(); - } + public List GetSteerWheels() + { + return _steerWheels.ToList(); + } private void SendTh(int i, float targetTh) { @@ -951,13 +1062,13 @@ namespace CommonUsage.Chassis return (0, rangeFront, rangeRear); } - private void AccumulateSpeed(int i, float v, bool axisDiff, Vector2 rotCenter, TimeSpan? deltaTime = null) - { - _targetSpeeds[i] = v; - var speedSign = Math.Sign(_targetSpeeds[i] - _sendSpeeds[i]); - var acc = Math.Abs(_targetSpeeds[i]) > Math.Abs(_sendSpeeds[i]) ? AccPerSecond : DeAccPerSecond; - _sendSpeeds[i] += speedSign * Math.Min(Math.Abs(_targetSpeeds[i] - _sendSpeeds[i]), - acc * (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds); + private void AccumulateSpeed(int i, float v, bool axisDiff, Vector2 rotCenter, TimeSpan? deltaTime = null) + { + _targetSpeeds[i] = v; + var speedSign = Math.Sign(_targetSpeeds[i] - _sendSpeeds[i]); + var acc = Math.Abs(_targetSpeeds[i]) > Math.Abs(_sendSpeeds[i]) ? AccPerSecond : DeAccPerSecond; + _sendSpeeds[i] += speedSign * Math.Min(Math.Abs(_targetSpeeds[i] - _sendSpeeds[i]), + acc * (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds); if (_steerWheels[i] is DiffSteerWheel dsw) { if (axisDiff) @@ -981,76 +1092,76 @@ namespace CommonUsage.Chassis _steerWheels[i].WriteSpeed(_sendSpeeds[i]); } - if (Debug) - Console.WriteLine($"Ackermann wheel{i}: target:{_targetSpeeds[i]:0.00},send:{_sendSpeeds[i]:0.0}"); - } + if (Debug) + Console.WriteLine($"Ackermann wheel{i}: target:{_targetSpeeds[i]:0.00},send:{_sendSpeeds[i]:0.0}"); + } - private void CalculateAxes() - { - var axes = _steerWheels - .Select(sw => (sw, Vector2.Dot(sw.Position, new Vector2(1, 0)))) - .OrderByDescending(ax => ax.Item2).ToList(); - _wheelBases = axes.Select(ax => ax.Item2).ToList(); - _steerWheels = axes.Select(ax => ax.sw).ToList(); - GeometricControlPoints = new List() - { - new (new Vector2(ControlPointRadius, 0)), - new (new Vector2(-ControlPointRadius, 0)) - }; - _frontBase = _wheelBases.First(); - _rearBase = _wheelBases.Last(); - } + private void CalculateAxes() + { + var axes = _steerWheels + .Select(sw => (sw, Vector2.Dot(sw.Position, new Vector2(1, 0)))) + .OrderByDescending(ax => ax.Item2).ToList(); + _wheelBases = axes.Select(ax => ax.Item2).ToList(); + _steerWheels = axes.Select(ax => ax.sw).ToList(); + GeometricControlPoints = new List() + { + new (new Vector2(ControlPointRadius, 0)), + new (new Vector2(-ControlPointRadius, 0)) + }; + _frontBase = _wheelBases.First(); + _rearBase = _wheelBases.Last(); + } - private List _steerWheels = new (); - private float _frontBase, _rearBase; - private List _wheelBases; - private List _sendSpeeds; - private List _targetSpeeds; - private List _sendAngle; + private List _steerWheels = new(); + private float _frontBase, _rearBase; + private List _wheelBases; + private List _sendSpeeds; + private List _targetSpeeds; + private List _sendAngle; private List _debugSpeeds; private DateTime _sendMotionDetailLastLog = DateTime.MinValue; private DateTime _geometricComputeLastLog = DateTime.MinValue; - private List _tmpSpeeds; - // add TimeStamp, prevent the wheel from swaying. - // for example, target angle is 90, current wheel is around 0. if no TimeStamp, - // wheel will swing between 90 and -90. - private List _wheelDirs; + private List _tmpSpeeds; + // add TimeStamp, prevent the wheel from swaying. + // for example, target angle is 90, current wheel is around 0. if no TimeStamp, + // wheel will swing between 90 and -90. + private List _wheelDirs; - // call this before a new continuous motion happens - private void ResetMotionState() - { - LastMoveTime = DateTime.Now; - GoingWheelAligned = false; - _wheelDirs = Enumerable.Repeat(1, _steerWheels.Count).ToList(); - } + // call this before a new motion sequence happens + private void ResetMotionState() + { + LastMoveTime = DateTime.Now; + GoingWheelAligned = false; + _wheelDirs = Enumerable.Repeat(1, _steerWheels.Count).ToList(); + } public void AddTestFunction() { } - /// - /// 得到相对舵轮在车体坐标系下的分解速度 - /// - /// - /// - /// - /// 单位为°/s - /// + /// + /// 得到相对舵轮在车体坐标系下的分解速度 + /// + /// + /// + /// + /// 单位为°/s + /// private Vector2 VectorVelocity(Vector2 pos, float vx, float vy, float vth, int i) { var vRotX = -vth / 180 * (float)Math.PI * pos.Y / 1000; var vRotY = vth / 180 * (float)Math.PI * pos.X / 1000; return new Vector2(vx + vRotX, vy + vRotY); } - /// - /// 获得车轮应该打的角度和速度 - /// - /// - /// - /// - /// 单位为°/s - /// + /// + /// 获得车轮应该打的角度和速度 + /// + /// + /// + /// + /// 单位为°/s + /// private (float angle, float speed) AngleAndSpeed(Vector2 pos, float vx, float vy, float vth, int i) { var v = VectorVelocity(pos, vx, vy, vth, i); @@ -1061,13 +1172,33 @@ namespace CommonUsage.Chassis /// 原地旋转时舵角误差对应的速度衰减宽度，单位为度。 /// public float SteeringAlignmentSigmaDegrees { get; set; } = 8f; + /// + /// 单轮速度低于此值时认为其运动方向无意义，单位为m/s。 + /// + public float WheelDirectionDeadbandMetersPerSecond { get; set; } = 0.005f; private bool XYThActive = false; private bool _xyThWheelsAligned = false; private DateTime _xyThDiagnosticsLastTime = DateTime.MinValue; - public bool SendXYThSpeed(float vx, float vy, float vth, TimeSpan? deltaTime = null) + /// + /// 下发车体二维速度，并根据舵轮机械角度误差进行高斯降速。 + /// 一段运动开始时必须先等待全部舵轮到位；运动过程中舵角误差越大， + /// 四轮驱动速度的统一缩放比例越小，适合作为默认安全接口。 + /// vx、vy单位为m/s，vth单位为°/s。 + /// + public bool SendXYThSpeed( + float vx, + float vy, + float vth, + TimeSpan? deltaTime = null) { - if (!Valid) return FailMotionDecomposition("SendXYThSpeed", "invalid chassis", deltaTime); + const string operationName = "SendXYThSpeed"; + + if (!Valid) + return FailMotionDecomposition( + operationName, + "invalid chassis", + deltaTime); if (Math.Abs(vx) < 1e-6f && Math.Abs(vy) < 1e-6f && Math.Abs(vth) < 1e-6f) { @@ -1080,18 +1211,19 @@ namespace CommonUsage.Chassis return true; } - if (!XYThActive) + if (!XYThActive) { ResetMotionState(); _xyThWheelsAligned = false; } - XYThActive = true; + XYThActive = true; GoingActive = false; RotatingActive = false; float[] sendSpeed = new float[_steerWheels.Count]; var allWheelsAligned = true; var maximumAngleError = 0f; + var alignmentSpeedScale = 1f; const float initialAlignmentToleranceDegrees = 2f; var writeDiagnostics = Debug && @@ -1102,31 +1234,59 @@ namespace CommonUsage.Chassis { var sw = _steerWheels[i]; var (angle, speed) = AngleAndSpeed(sw.Position, vx, vy, vth, i); + // 单轮合成速度接近零时，运动方向没有物理意义。 + // 此时不重新计算和下发舵角，保持上一目标舵角，轮速降为零。 + var directionDeadband = Math.Max( + WheelDirectionDeadbandMetersPerSecond, + 0f); + + if (speed < directionDeadband) + { + sendSpeed[i] = 0f; + + if (writeDiagnostics) + { + Hedingben.ToastText( + $"hold-angle speed:{speed:F4} deadband:{directionDeadband:F4}", + $"{operationName}-{i}"); + } + + continue; + } var actualTh = sw.ReadAngle(); + if (float.IsNaN(actualTh) || float.IsInfinity(actualTh)) { return FailMotionDecomposition( - "SendXYThSpeed", + operationName, $"wheel {i} angle feedback is invalid: {actualTh}", deltaTime); } - if (!TryResolveWheelAngle(i, CommonMath.ThDiff(angle, sw.ZeroDirection), "SendXYThSpeed", + if (!TryResolveWheelAngle(i, CommonMath.ThDiff(angle, sw.ZeroDirection), operationName, out var useAngle, out var dir, out var resolveReason)) - return FailMotionDecomposition("SendXYThSpeed", resolveReason, deltaTime); + return FailMotionDecomposition(operationName, resolveReason, deltaTime); speed *= dir; _wheelDirs[i] = dir; - + sendSpeed[i] = speed; - if (speed!=0) - SendTh(i, useAngle); + SendTh(i, useAngle); // 这里比较受机械限位约束的实际舵角，不使用圆周最短角。 var angleError = Math.Abs(_sendAngle[i] - actualTh); maximumAngleError = Math.Max(maximumAngleError, angleError); + + alignmentSpeedScale = Math.Min( + alignmentSpeedScale, + CommonMath.gaussmf( + angleError, + Math.Max( + SteeringAlignmentSigmaDegrees, + 0.1f), + 0)); if (angleError > initialAlignmentToleranceDegrees) { @@ -1136,37 +1296,40 @@ namespace CommonUsage.Chassis if (writeDiagnostics) { Hedingben.ToastText( - $"ready:{_xyThWheelsAligned} err:{angleError:F1} " + + $"ready:{_xyThWheelsAligned} " + + $"err:{angleError:F1} scale:{alignmentSpeedScale:F2} " + $"s:{speed:F3} th:{_sendAngle[i]:F1} actualTh:{actualTh:F1}", - $"SendXYThSpeed-{i}"); + $"{operationName}-{i}"); } } - // 仅在一段XYTh运动刚开始时等待舵轮到位。 - // 连续运动开始后，正常改变vx/vy/vth时允许舵轮边转、车辆边走， - // 避免每次打方向都重新把驱动速度压到零。 + // 一段XYTh运动刚开始时必须等待全部舵轮到位。 if (!_xyThWheelsAligned && allWheelsAligned) { _xyThWheelsAligned = true; } - var driveEnabled = _xyThWheelsAligned; + var driveScale = _xyThWheelsAligned + ? alignmentSpeedScale + : 0f; for (var i = 0; i < _steerWheels.Count; i++) AccumulateSpeed( i, - driveEnabled ? sendSpeed[i] : 0f, + driveScale * + sendSpeed[i], false, - new Vector2(0f,0f), + new Vector2(0f, 0f), deltaTime); if (writeDiagnostics) { _xyThDiagnosticsLastTime = DateTime.Now; Hedingben.ToastText( - $"ready:{_xyThWheelsAligned} maxErr:{maximumAngleError:F1} " + + $"ready:{_xyThWheelsAligned} " + + $"maxErr:{maximumAngleError:F1} scale:{driveScale:F2} " + $"cmd:({vx:F3},{vy:F3},{vth:F1})", - "SendXYThSpeed-alignment"); + $"{operationName}-alignment"); } //todo 计算rotCenter填入 LastMoveTime = DateTime.Now; @@ -1183,7 +1346,7 @@ namespace CommonUsage.Chassis { var sw1 = _steerWheels[i]; Vector2 a = sw1.Position / 1000; - //var tha = _sendAngle[i] / 180 * (float)Math.PI; + //var tha = _sendAngle[i] / 180 * (float)Math.PI; var tha = isActual ? sw1.ReadAngle() / 180 * (float)Math.PI : _sendAngle[i] / 180 * (float)Math.PI; //var speeda = _sendSpeeds[i]; //var speeda = isActual ? sw1.ReadSpeed() : _sendSpeeds[i]; @@ -1195,7 +1358,7 @@ namespace CommonUsage.Chassis { var sw2 = _steerWheels[j]; Vector2 b = sw2.Position / 1000; - //var thb = _sendAngle[j] / 180 * (float)Math.PI; + //var thb = _sendAngle[j] / 180 * (float)Math.PI; var thb = isActual ? sw2.ReadAngle() / 180 * (float)Math.PI : _sendAngle[j] / 180 * (float)Math.PI; //var speedb = _sendSpeeds[j]; //var speedb = isActual ? sw2.ReadSpeed() : _sendSpeeds[j]; @@ -1208,7 +1371,7 @@ namespace CommonUsage.Chassis } } return new CarSpeed() - { Vx = vx.Average(), Vy = vy.Average(), Vw = (float)(vth.Average() / Math.PI * 180f) }; + { Vx = vx.Average(), Vy = vy.Average(), Vw = (float)(vth.Average() / Math.PI * 180f) }; } private static (float, float, float) CenterVelocityFromPoints(Vector2 a, Vector2 va, Vector2 b, Vector2 vb) diff --git a/CommonUsage-MultiVehicleSync/commonusage/CommonUsage.csproj b/CommonUsage-MultiVehicleSync/commonusage/CommonUsage.csproj index d5f4d92..dc93ad8 100644 --- a/CommonUsage-MultiVehicleSync/commonusage/CommonUsage.csproj +++ b/CommonUsage-MultiVehicleSync/commonusage/CommonUsage.csproj @@ -2,6 +2,8 @@ netstandard2.0 + CommonUsage + CommonUsage diff --git a/MedullaAdapter/DiverCartDefinition.cs b/MedullaAdapter/DiverCartDefinition.cs index a8a0692..f78a75b 100644 --- a/MedullaAdapter/DiverCartDefinition.cs +++ b/MedullaAdapter/DiverCartDefinition.cs @@ -70,6 +70,9 @@ namespace MedullaAdapter [AsInitParam(desc = "手动控制夹臂速度系数")] public float ManualArmSpeedFac = 1.0f; [AsInitParam(desc = "遥控转弯舵角同步限速宽度，单位为度")] public float ManualSteeringAlignmentSigmaDegrees = 8.0f; + [AsInitParam(desc = "轮速诊断日志相对目录")] + public string WheelSpeedDiagnosticDirectory = + @"logs\wheel-speed"; [AsInitParam(desc = "左夹臂低限位")][AsLowerIO] public int LeftArmLowerPos = -10000; [AsInitParam(desc = "左夹臂高限位")][AsLowerIO] public int LeftArmUpperPos = 5927610; [AsInitParam(desc = "右夹臂低限位")][AsLowerIO] public int RightArmLowerPos = -17295; @@ -91,6 +94,10 @@ namespace MedullaAdapter [IOObjectMonitor(desc = "右后右轮PID修正后速度")] public float SpeedRRR; [IOObjectMonitor(desc = "灯光模式")] public int LightMode = 0; [IOObjectMonitor(desc = "实体遥控器当前速度倍率")] public float TransmitterSpeed = 0.3f; + [IOObjectMonitor(desc = "轮速诊断记录已启用")] + public bool WheelSpeedDiagnosticEnabled; + [IOObjectMonitor(desc = "轮速诊断记录状态")] + public string WheelSpeedDiagnosticStatus = "未启动"; [IOObjectMonitor(desc = "左前左驱动器远程帧701")] public byte LFLRemoteCode = 0; [IOObjectMonitor(desc = "左前右驱动器远程帧702")] public byte LFRRemoteCode = 0; [IOObjectMonitor(desc = "右前左驱动器远程帧703")] public byte RFLRemoteCode = 0; @@ -116,6 +123,22 @@ namespace MedullaAdapter { DisableFromM = true; } + + // M层诊断：请求开始保存CAN轮速事件和底盘周期快照。 + [IOObjectUtility] + public void StartWheelSpeedDiagnostic() + { + WheelSpeedDiagnosticEnabled = true; + WheelSpeedDiagnosticStatus = "等待创建日志文件"; + } + + // M层诊断：请求停止轮速记录并刷新CSV文件。 + [IOObjectUtility] + public void StopWheelSpeedDiagnostic() + { + WheelSpeedDiagnosticEnabled = false; + WheelSpeedDiagnosticStatus = "等待停止并刷新日志"; + } #endregion public override void CommunicationInit() @@ -234,10 +257,8 @@ namespace MedullaAdapter Math.Sign(x); var steeringDegrees = -normalizedSteering * MaxManualTheta; - var omega = CalculateManualOmega( - speed, - steeringDegrees, - adapter.HalfWheelBaseMeters); + var frontTh = steeringDegrees; + var rearTh = -steeringDegrees; ManualMode = (int)mode; switch (mode) @@ -245,27 +266,96 @@ namespace MedullaAdapter case ManualControlMode.Normal: // 普通模式统一使用车体速度命令： // X向前，行驶中连续改变角速度时舵轮边转、车辆边走。 - SendBodyCommand( - vx: speed, - vy: 0.0, - omegaRadiansPerSecond: omega, - interval); + // SendBodyCommand( + // vx: speed, + // vy: 0.0, + // omegaRadiansPerSecond: omega, + // interval); + Chassis.SendMotion( + speed, + frontTh, + rearTh, + interval); break; case ManualControlMode.Crab: - SendBodyCommand(vx: 0.0, vy: speed, omegaRadiansPerSecond: omega, interval); + // 舵轮机械范围为[-120°,120°]。 + // 蟹行后虚拟轴距由原车宽度决定，比正常模式轴距短。 + // 按几何比例缩小转角，使相同摇杆输入获得接近一致的曲率。 + var normalSteeringRadians = + steeringDegrees * + Math.PI / 180.0; + var geometryRatio = + adapter.HalfTrackWidthMeters / + adapter.HalfWheelBaseMeters; + + // +90°运动坐标系已经把虚拟左侧映射为车体后方， + // 此处保持普通模式的转向符号，避免再次取反导致左右颠倒。 + var crabSteeringRadians = + Math.Atan( + geometryRatio * + Math.Tan( + normalSteeringRadians)); + + // 蟹行转角最终限制为±30°，为±120°机械舵角保留余量。 + var maximumCrabSteeringRadians = + 30.0 * Math.PI / 180.0; + crabSteeringRadians = Math.Max( + -maximumCrabSteeringRadians, + Math.Min( + maximumCrabSteeringRadians, + crabSteeringRadians)); + + // 将车体左侧作为虚拟阿克曼车头，并在该运动坐标系中 + // 复用与普通模式相同的SendMotion前后控制点解算。 + if (!adapter.SendVirtualAckermannMotion( + motionDirectionRadians: + Math.PI / 2.0, + speedMetersPerSecond: + speed, + steeringRadians: + crabSteeringRadians, + interval)) + { + adapter.StopImmediately(); + + Console.WriteLine( + "蟹行SendMotion命令分解失败，车辆已经停车：" + + adapter.LastFailureReason); + } break; case ManualControlMode.Spin: - // 自转时speed表示最外侧舵轮的目标切向速度。 - // 根据v=omega*r换算角速度，不能把m/s与deg/s直接相乘。 - var spinOmega = + // 摇杆处于中位时只清零驱动速度，保持已经准备好的 + // 自转舵角；下次推动摇杆时仍会重新检查实际舵角。 + if (Math.Abs(speed) < 1e-6f) + { + adapter + .StopXYThDrivePreserveSteeringState(); + break; + } + + // 自转时speed表示最外侧舵轮中心的目标切向速度， + // 根据v=omega*r换算为SendXYThSpeed需要的角速度。 + var spinOmegaRadiansPerSecond = speed / adapter.MaximumWheelRadiusMeters; - SendBodyCommand( - vx: 0.0, - vy: 0.0, - omegaRadiansPerSecond: spinOmega, - interval); + // 普通安全版SendXYThSpeed只下发角速度， + // 四轮实际舵角未到位时不会开放驱动速度。 + if (!adapter.Send( + new ChassisCommand( + CarNum, + new Twist2D( + 0.0, + 0.0, + spinOmegaRadiansPerSecond)), + interval)) + { + adapter.StopImmediately(); + + Console.WriteLine( + "SendXYThSpeed原地自转命令分解失败，车辆已经停车：" + + adapter.LastFailureReason); + } break; default: ManualMode = -1; @@ -294,6 +384,10 @@ namespace MedullaAdapter if (_pendingManualMode != mode) { adapter.StopImmediately(); + // 所有模式的准备角度均按真实机械舵角表达； + // 先退出上一模式的虚拟运动坐标系，再执行预对齐。 + adapter.ResetToBodyFrame(); + _activeManualMode = null; var preparationAccepted = mode switch { @@ -354,24 +448,33 @@ namespace MedullaAdapter if (!aligned) return false; + if (mode == ManualControlMode.Spin) + { + // 四轮实际舵角确认到位后只交接一次，保留PrepareSpin + // 选定的机械舵角和轮速方向，避免首条XYTh命令重新选角。 + if (!adapter.AdoptPreparedSpinForXYTh( + toleranceRadians)) + { + return false; + } + } + // 蟹行轮子在真实车体系中到达机械+90°后， + // 再将车体左侧激活为SendMotion的虚拟X正方向。 + else if (mode == ManualControlMode.Crab) + { + adapter.ActivateMotionFrame( + Math.PI / 2.0); + } + else + { + adapter.ResetToBodyFrame(); + } + _activeManualMode = mode; _pendingManualMode = null; return true; } - private double CalculateManualOmega( - float speed, - float steeringDegrees, - double halfWheelBaseMeters) - { - var steeringRadians = - steeringDegrees * Math.PI / 180.0; - - return speed * - Math.Tan(steeringRadians) / - Math.Max(halfWheelBaseMeters, 0.01); - } - internal void SendBodyCommand(double vx, double vy, double omegaRadiansPerSecond, TimeSpan? interval = null) { var adapter = GetChassisAdapter(); diff --git a/MedullaAdapter/MCURoutine.cs b/MedullaAdapter/MCURoutine.cs index ab9171f..1d38042 100644 --- a/MedullaAdapter/MCURoutine.cs +++ b/MedullaAdapter/MCURoutine.cs @@ -4,6 +4,7 @@ using FundamentalLib; using MCUSerialBridgeCLR; using System; using System.Collections.Generic; +using System.IO; namespace MedullaAdapter { @@ -26,6 +27,9 @@ namespace MedullaAdapter private bool io_bit4 = false;//黄灯 private const byte BatteryPortIndex = 3; private static readonly byte[] BatteryRequest = BuildBatteryRequest(); + private readonly WheelSpeedDiagnosticLogger + _wheelSpeedLogger = + new WheelSpeedDiagnosticLogger(); // M层单车底盘：将车轮线速度换算为驱动电机转速。 private static float ConvertMps2Rpm(float mps) @@ -50,6 +54,8 @@ namespace MedullaAdapter // M层硬件主循环：交换IO、发送轮组指令并更新车辆反馈状态。 public override void Operation(int iteration) { + UpdateWheelSpeedDiagnosticState(); + if (_lastIteration != iteration) { _lastIteration = iteration; @@ -158,6 +164,7 @@ namespace MedullaAdapter cart.ActualSpeedLeftRear = (cart.ActualSpeedLeftRearLeft + cart.ActualSpeedLeftRearRight) / 2; cart.ActualSpeedRightFront = (cart.ActualSpeedRightFrontLeft + cart.ActualSpeedRightFrontRight) / 2; cart.ActualSpeedRightRear = (cart.ActualSpeedRightRearLeft + cart.ActualSpeedRightRearRight) / 2; + _wheelSpeedLogger.RecordSnapshot(cart); // M层CAN辅助：封装本周期驱动器CAN发送参数。 MCUSerialBridgeError SendCan(byte port, ushort standardId, byte[] payload, bool RTR = false, uint timeout = 2) { @@ -264,8 +271,9 @@ namespace MedullaAdapter { _operationTime++; return; - } } + } + // 没有错误 没有节点保护 就发06 07 0F使能 else if (_operationTime == 4) { @@ -428,6 +436,79 @@ namespace MedullaAdapter } + // M层诊断：根据界面开关创建或关闭本次轮速CSV记录。 + private void UpdateWheelSpeedDiagnosticState() + { + if (cart == null) + return; + + if (cart.WheelSpeedDiagnosticEnabled) + { + if (_wheelSpeedLogger.IsRunning) + return; + + try + { + var configuredDirectory = + string.IsNullOrWhiteSpace( + cart.WheelSpeedDiagnosticDirectory) + ? @"logs\wheel-speed" + : cart.WheelSpeedDiagnosticDirectory; + + var logDirectory = + Path.IsPathRooted(configuredDirectory) + ? configuredDirectory + : Path.Combine( + AppContext.BaseDirectory, + configuredDirectory); + + _wheelSpeedLogger.Start( + logDirectory, + cart.CarNum); + + cart.WheelSpeedDiagnosticStatus = + "记录中：" + + _wheelSpeedLogger.SnapshotLogPath; + } + catch (Exception ex) + { + cart.WheelSpeedDiagnosticEnabled = + false; + cart.WheelSpeedDiagnosticStatus = + "启动失败：" + ex.Message; + + Console.WriteLine( + "轮速诊断启动失败：" + + ex.Message); + } + + return; + } + + if (!_wheelSpeedLogger.IsRunning) + return; + + try + { + var snapshotPath = + _wheelSpeedLogger.SnapshotLogPath; + + _wheelSpeedLogger.Stop(); + + cart.WheelSpeedDiagnosticStatus = + "已保存：" + snapshotPath; + } + catch (Exception ex) + { + cart.WheelSpeedDiagnosticStatus = + "停止失败：" + ex.Message; + + Console.WriteLine( + "轮速诊断停止失败：" + + ex.Message); + } + } + // M层CAN安全：判断单个驱动节点是否处于可运行状态。 private static bool IsNodeOperational(byte remoteCode) { @@ -601,7 +682,13 @@ namespace MedullaAdapter var payload = msg.Payload; if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); - cart.ActualSpeedLeftFrontLeft = ConvertRpm2Mps(rpm); + var speed = ConvertRpm2Mps(rpm); + cart.ActualSpeedLeftFrontLeft = speed; + _wheelSpeedLogger.RecordCanFeedback( + 0x281, + "LFL", + rpm, + speed); cart.LFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f); }, [0x282] = (msg) => @@ -610,7 +697,13 @@ namespace MedullaAdapter var payload = msg.Payload; if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); - cart.ActualSpeedLeftFrontRight = -ConvertRpm2Mps(rpm); + var speed = -ConvertRpm2Mps(rpm); + cart.ActualSpeedLeftFrontRight = speed; + _wheelSpeedLogger.RecordCanFeedback( + 0x282, + "LFR", + rpm, + speed); cart.LFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f); }, [0x283] = (msg) => @@ -619,7 +712,13 @@ namespace MedullaAdapter var payload = msg.Payload; if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); - cart.ActualSpeedRightFrontLeft = ConvertRpm2Mps(rpm); + var speed = ConvertRpm2Mps(rpm); + cart.ActualSpeedRightFrontLeft = speed; + _wheelSpeedLogger.RecordCanFeedback( + 0x283, + "RFL", + rpm, + speed); cart.RFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f); }, [0x284] = (msg) => @@ -628,7 +727,13 @@ namespace MedullaAdapter var payload = msg.Payload; if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); - cart.ActualSpeedRightFrontRight = -ConvertRpm2Mps(rpm); + var speed = -ConvertRpm2Mps(rpm); + cart.ActualSpeedRightFrontRight = speed; + _wheelSpeedLogger.RecordCanFeedback( + 0x284, + "RFR", + rpm, + speed); cart.RFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f); }, [0x285] = (msg) => @@ -637,7 +742,13 @@ namespace MedullaAdapter var payload = msg.Payload; if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); - cart.ActualSpeedLeftRearLeft = ConvertRpm2Mps(rpm); + var speed = ConvertRpm2Mps(rpm); + cart.ActualSpeedLeftRearLeft = speed; + _wheelSpeedLogger.RecordCanFeedback( + 0x285, + "LRL", + rpm, + speed); cart.LRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f); }, [0x286] = (msg) => @@ -646,7 +757,13 @@ namespace MedullaAdapter var payload = msg.Payload; if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); - cart.ActualSpeedLeftRearRight = -ConvertRpm2Mps(rpm); + var speed = -ConvertRpm2Mps(rpm); + cart.ActualSpeedLeftRearRight = speed; + _wheelSpeedLogger.RecordCanFeedback( + 0x286, + "LRR", + rpm, + speed); cart.LRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f); }, [0x287] = (msg) => @@ -655,7 +772,13 @@ namespace MedullaAdapter var payload = msg.Payload; if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); - cart.ActualSpeedRightRearLeft = ConvertRpm2Mps(rpm); + var speed = ConvertRpm2Mps(rpm); + cart.ActualSpeedRightRearLeft = speed; + _wheelSpeedLogger.RecordCanFeedback( + 0x287, + "RRL", + rpm, + speed); cart.RRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f); }, [0x288] = (msg) => @@ -664,7 +787,13 @@ namespace MedullaAdapter var payload = msg.Payload; if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); - cart.ActualSpeedRightRearRight = -ConvertRpm2Mps(rpm); + var speed = -ConvertRpm2Mps(rpm); + cart.ActualSpeedRightRearRight = speed; + _wheelSpeedLogger.RecordCanFeedback( + 0x288, + "RRR", + rpm, + speed); cart.RRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f); }, [0x289] = (msg) => diff --git a/MedullaAdapter/WheelSpeedDiagnosticLogger.cs b/MedullaAdapter/WheelSpeedDiagnosticLogger.cs new file mode 100644 index 0000000..3f341a5 --- /dev/null +++ b/MedullaAdapter/WheelSpeedDiagnosticLogger.cs @@ -0,0 +1,362 @@ +using System; +using System.Collections.Concurrent; +using System.Diagnostics; +using System.Globalization; +using System.IO; +using System.Text; +using System.Threading; + +namespace MedullaAdapter +{ + /// + /// 在后台保存驱动器CAN速度事件和底盘周期快照，避免文件IO阻塞CAN回调。 + /// + internal sealed class WheelSpeedDiagnosticLogger : IDisposable + { + private readonly struct LogRecord + { + public LogRecord(bool isCanEvent, string line) + { + IsCanEvent = isCanEvent; + Line = line; + } + + public bool IsCanEvent { get; } + + public string Line { get; } + } + + private const int MaximumQueuedRecords = 100000; + private const double SnapshotIntervalMilliseconds = 20.0; + private readonly ConcurrentQueue _records = new(); + private readonly AutoResetEvent _recordsAvailable = new(false); + private readonly object _lifecycleLock = new(); + private Stopwatch _stopwatch; + private Thread _writerThread; + private StreamWriter _canWriter; + private StreamWriter _snapshotWriter; + private volatile bool _isRunning; + private int _queuedRecordCount; + private long _receiveSequence; + private long _droppedRecordCount; + private double _lastSnapshotMilliseconds = double.NegativeInfinity; + + public bool IsRunning => _isRunning; + + public string CanLogPath { get; private set; } = ""; + + public string SnapshotLogPath { get; private set; } = ""; + + /// + /// 创建本次诊断的两个CSV文件并启动后台写入线程。 + /// + public void Start(string directory, int carNumber) + { + lock (_lifecycleLock) + { + if (_isRunning) + return; + + if (string.IsNullOrWhiteSpace(directory)) + throw new ArgumentException( + "轮速诊断目录不能为空。", + nameof(directory)); + + Directory.CreateDirectory(directory); + + var filePrefix = + $"{DateTime.Now:yyyyMMdd_HHmmss_fff}_Car{carNumber}"; + + CanLogPath = Path.Combine( + directory, + $"{filePrefix}_can.csv"); + + SnapshotLogPath = Path.Combine( + directory, + $"{filePrefix}_snapshot.csv"); + + _canWriter = CreateWriter(CanLogPath); + _snapshotWriter = CreateWriter(SnapshotLogPath); + + _canWriter.WriteLine( + "ElapsedMs,ReceiveSequence,CanId,MotorName,RawRpm,SpeedMps"); + + _snapshotWriter.WriteLine( + "ElapsedMs,CarNum,ManualMode,SendThresSpeed," + + "CmdLFL,CmdLFR,CmdLRL,CmdLRR,CmdRFL,CmdRFR,CmdRRL,CmdRRR," + + "PidLFL,PidLFR,PidLRL,PidLRR,PidRFL,PidRFR,PidRRL,PidRRR," + + "ActualLFL,ActualLFR,ActualLRL,ActualLRR,ActualRFL,ActualRFR,ActualRRL,ActualRRR," + + "ActualLeftFront,ActualLeftRear,ActualRightFront,ActualRightRear," + + "TargetThLeftFront,TargetThLeftRear,TargetThRightFront,TargetThRightRear," + + "ActualThLeftFront,ActualThLeftRear,ActualThRightFront,ActualThRightRear"); + + while (_records.TryDequeue(out _)) + { + } + + _queuedRecordCount = 0; + _receiveSequence = 0; + _droppedRecordCount = 0; + _lastSnapshotMilliseconds = + double.NegativeInfinity; + _stopwatch = Stopwatch.StartNew(); + _isRunning = true; + + _writerThread = new Thread(WriterLoop) + { + IsBackground = true, + Name = "WheelSpeedDiagnosticWriter" + }; + _writerThread.Start(); + } + } + + /// + /// 停止记录并等待队列中的诊断数据写入磁盘。 + /// + public void Stop() + { + Thread writerThread; + + lock (_lifecycleLock) + { + if (!_isRunning && + _writerThread == null) + return; + + _isRunning = false; + writerThread = _writerThread; + _recordsAvailable.Set(); + } + + writerThread?.Join(3000); + + lock (_lifecycleLock) + { + _canWriter?.Flush(); + _snapshotWriter?.Flush(); + _canWriter?.Dispose(); + _snapshotWriter?.Dispose(); + _canWriter = null; + _snapshotWriter = null; + _writerThread = null; + _stopwatch?.Stop(); + } + } + + /// + /// 将一帧驱动器速度反馈加入内存队列，不在CAN回调中执行文件写入。 + /// + public void RecordCanFeedback( + ushort canId, + string motorName, + float rawRpm, + float speedMetersPerSecond) + { + if (!_isRunning) + return; + + var elapsedMilliseconds = + _stopwatch.Elapsed.TotalMilliseconds; + var receiveSequence = + Interlocked.Increment( + ref _receiveSequence); + + var line = string.Join( + ",", + Format(elapsedMilliseconds), + receiveSequence.ToString( + CultureInfo.InvariantCulture), + $"0x{canId:X3}", + motorName, + Format(rawRpm), + Format(speedMetersPerSecond)); + + Enqueue(new LogRecord( + isCanEvent: true, + line)); + } + + /// + /// 按最多50Hz记录一帧控制命令、PID输出、CAN反馈和舵角快照。 + /// + public void RecordSnapshot( + DiverCartDefinition cart) + { + if (!_isRunning || cart == null) + return; + + var elapsedMilliseconds = + _stopwatch.Elapsed.TotalMilliseconds; + + if (elapsedMilliseconds - + _lastSnapshotMilliseconds < + SnapshotIntervalMilliseconds) + { + return; + } + + _lastSnapshotMilliseconds = + elapsedMilliseconds; + + var line = string.Join( + ",", + Format(elapsedMilliseconds), + cart.CarNum.ToString( + CultureInfo.InvariantCulture), + Format(cart.ManualMode), + Format(cart.SendThresSpeed), + Format(cart.SpeedLeftFrontLeft), + Format(cart.SpeedLeftFrontRight), + Format(cart.SpeedLeftRearLeft), + Format(cart.SpeedLeftRearRight), + Format(cart.SpeedRightFrontLeft), + Format(cart.SpeedRightFrontRight), + Format(cart.SpeedRightRearLeft), + Format(cart.SpeedRightRearRight), + Format(cart.SpeedLFL), + Format(cart.SpeedLFR), + Format(cart.SpeedLRL), + Format(cart.SpeedLRR), + Format(cart.SpeedRFL), + Format(cart.SpeedRFR), + Format(cart.SpeedRRL), + Format(cart.SpeedRRR), + Format(cart.ActualSpeedLeftFrontLeft), + Format(cart.ActualSpeedLeftFrontRight), + Format(cart.ActualSpeedLeftRearLeft), + Format(cart.ActualSpeedLeftRearRight), + Format(cart.ActualSpeedRightFrontLeft), + Format(cart.ActualSpeedRightFrontRight), + Format(cart.ActualSpeedRightRearLeft), + Format(cart.ActualSpeedRightRearRight), + Format(cart.ActualSpeedLeftFront), + Format(cart.ActualSpeedLeftRear), + Format(cart.ActualSpeedRightFront), + Format(cart.ActualSpeedRightRear), + Format(cart.ThLeftFront), + Format(cart.ThLeftRear), + Format(cart.ThRightFront), + Format(cart.ThRightRear), + Format(cart.ActualThLeftFront), + Format(cart.ActualThLeftRear), + Format(cart.ActualThRightFront), + Format(cart.ActualThRightRear)); + + Enqueue(new LogRecord( + isCanEvent: false, + line)); + } + + private static StreamWriter CreateWriter( + string path) + { + return new StreamWriter( + path, + append: false, + new UTF8Encoding( + encoderShouldEmitUTF8Identifier: true), + bufferSize: 64 * 1024); + } + + private void Enqueue(LogRecord record) + { + var queuedCount = + Interlocked.Increment( + ref _queuedRecordCount); + + if (queuedCount > + MaximumQueuedRecords) + { + Interlocked.Decrement( + ref _queuedRecordCount); + Interlocked.Increment( + ref _droppedRecordCount); + return; + } + + _records.Enqueue(record); + _recordsAvailable.Set(); + } + + private void WriterLoop() + { + var lastFlushTime = DateTime.UtcNow; + + try + { + while (_isRunning || + !_records.IsEmpty) + { + var wroteAnyRecord = false; + + while (_records.TryDequeue( + out var record)) + { + Interlocked.Decrement( + ref _queuedRecordCount); + + if (record.IsCanEvent) + _canWriter.WriteLine(record.Line); + else + _snapshotWriter.WriteLine(record.Line); + + wroteAnyRecord = true; + } + + var shouldFlush = + wroteAnyRecord && + (DateTime.UtcNow - + lastFlushTime) + .TotalMilliseconds >= 500.0; + + if (shouldFlush) + { + _canWriter.Flush(); + _snapshotWriter.Flush(); + lastFlushTime = DateTime.UtcNow; + } + + if (!wroteAnyRecord) + _recordsAvailable.WaitOne(100); + } + + var dropped = + Interlocked.Read( + ref _droppedRecordCount); + + if (dropped > 0) + { + _canWriter.WriteLine( + $"# DroppedRecords={dropped}"); + _snapshotWriter.WriteLine( + $"# DroppedRecords={dropped}"); + } + + _canWriter.Flush(); + _snapshotWriter.Flush(); + } + catch (Exception ex) + { + // 后台日志失败不能终止车辆控制线程。 + Console.WriteLine( + "轮速诊断后台写入失败：" + + ex.Message); + } + } + + private static string Format( + double value) + { + return value.ToString( + "0.######", + CultureInfo.InvariantCulture); + } + + public void Dispose() + { + Stop(); + _recordsAvailable.Dispose(); + } + } +} diff --git a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll index 209c020..37c2382 100644 Binary files a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll and b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll differ diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll index 7b934b8..ac14237 100644 Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll differ diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb index aeb0ea0..500cf0c 100644 Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb differ diff --git a/MedullaAdapter/obj/MedullaAdapter.csproj.nuget.dgspec.json b/MedullaAdapter/obj/MedullaAdapter.csproj.nuget.dgspec.json index ca24af8..83c31fc 100644 --- a/MedullaAdapter/obj/MedullaAdapter.csproj.nuget.dgspec.json +++ b/MedullaAdapter/obj/MedullaAdapter.csproj.nuget.dgspec.json @@ -10,7 +10,7 @@ "projectUniqueName": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj", "projectName": "MedullaAdapter", "projectPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj", - "packagesPath": "C:\\Users\\admin\\.nuget\\packages\\", + "packagesPath": "C:\\Users\\CodexSandboxOffline\\.nuget\\packages\\", "outputPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\obj\\", "projectStyle": "PackageReference", "fallbackFolders": [ diff --git a/MedullaAdapter/obj/MedullaAdapter.csproj.nuget.g.props b/MedullaAdapter/obj/MedullaAdapter.csproj.nuget.g.props index 784b9d3..c5374a0 100644 --- a/MedullaAdapter/obj/MedullaAdapter.csproj.nuget.g.props +++ b/MedullaAdapter/obj/MedullaAdapter.csproj.nuget.g.props @@ -4,13 +4,13 @@ True NuGet $(MSBuildThisFileDirectory)project.assets.json - $(UserProfile)\.nuget\packages\ - C:\Users\admin\.nuget\packages\;C:\Program Files (x86)\Microsoft Visual Studio\Shared\NuGetPackages + C:\Users\CodexSandboxOffline\.nuget\packages\ + C:\Users\CodexSandboxOffline\.nuget\packages\;C:\Program Files (x86)\Microsoft Visual Studio\Shared\NuGetPackages PackageReference 6.14.3 - + \ No newline at end of file diff --git a/MedullaAdapter/obj/project.assets.json b/MedullaAdapter/obj/project.assets.json index f33687d..116af55 100644 --- a/MedullaAdapter/obj/project.assets.json +++ b/MedullaAdapter/obj/project.assets.json @@ -8,7 +8,7 @@ "net8.0": [] }, "packageFolders": { - "C:\\Users\\admin\\.nuget\\packages\\": {}, + "C:\\Users\\CodexSandboxOffline\\.nuget\\packages\\": {}, "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages": {} }, "project": { @@ -17,7 +17,7 @@ "projectUniqueName": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj", "projectName": "MedullaAdapter", "projectPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj", - "packagesPath": "C:\\Users\\admin\\.nuget\\packages\\", + "packagesPath": "C:\\Users\\CodexSandboxOffline\\.nuget\\packages\\", "outputPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\obj\\", "projectStyle": "PackageReference", "fallbackFolders": [ diff --git a/MedullaAdapter/obj/project.nuget.cache b/MedullaAdapter/obj/project.nuget.cache index 35309d3..9501177 100644 --- a/MedullaAdapter/obj/project.nuget.cache +++ b/MedullaAdapter/obj/project.nuget.cache @@ -1,6 +1,6 @@ { "version": 2, - "dgSpecHash": "4fABnQtycfA=", + "dgSpecHash": "b9v8vkN2ac8=", "success": true, "projectFilePath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj", "expectedPackageFiles": [], diff --git a/MedullaAdapter/ref/CommonUsage.dll b/MedullaAdapter/ref/CommonUsage.dll.legacy similarity index 100% rename from MedullaAdapter/ref/CommonUsage.dll rename to MedullaAdapter/ref/CommonUsage.dll.legacy diff --git a/Shared/MultiWheelChassisAdapter.cs b/Shared/MultiWheelChassisAdapter.cs index 1a0be56..cd1b96e 100644 --- a/Shared/MultiWheelChassisAdapter.cs +++ b/Shared/MultiWheelChassisAdapter.cs @@ -30,6 +30,12 @@ namespace MyParking.Shared /// public double HalfWheelBaseMeters { get; } + /// + /// 车体原点到最外侧舵轮中心的最大横向距离，单位为米。 + /// 对称四舵轮底盘中，它也是蟹行虚拟阿克曼模型的半轴距。 + /// + public double HalfTrackWidthMeters { get; } + /// /// Width of the steering-alignment speed gate, in degrees. /// @@ -58,19 +64,52 @@ namespace MyParking.Shared /// private void EnsureBodyFrameIsActive() { + EnsureMotionFrameIsActive(0.0); + } + + /// + /// 检查旧底盘当前是否处于指定的运动坐标系。 + /// motionDirectionRadians表示该运动系X轴在真实车体坐标系中的方向。 + /// + private void EnsureMotionFrameIsActive( + double motionDirectionRadians) + { + ValidateFinite( + motionDirectionRadians, + nameof(motionDirectionRadians)); + + var expectedBiasDegrees = + (float)( + -FrameTransform2D.NormalizeAngle( + motionDirectionRadians) * + RadiansToDegrees); var bias = _chassis.GetOriginBias(); + var angleErrorDegrees = + NormalizeDegrees( + bias.Z - expectedBiasDegrees); if (Math.Abs(bias.X) <= BiasTolerance && Math.Abs(bias.Y) <= BiasTolerance && - Math.Abs(bias.Z) <= BiasTolerance) + Math.Abs(angleErrorDegrees) <= + BiasTolerance) { return; } throw new InvalidOperationException( - "MultiWheelChassis的坐标偏置在适配器创建后被修改。" + - $"当前偏置为X={bias.X}, Y={bias.Y}, Th={bias.Z}°。" + - "请不要再调用DirectionAngle或SetOriginBias控制蟹行。"); + "MultiWheelChassis当前运动坐标系与命令不一致。" + + $"当前偏置为X={bias.X}, Y={bias.Y}, Th={bias.Z}°，" + + $"期望Th={expectedBiasDegrees}°。"); + } + + /// + /// 将角度归一化到[-180°,180°]附近。 + /// + private static float NormalizeDegrees(float degrees) + { + return (float)( + degrees - + Math.Round(degrees / 360.0) * 360.0); } /// /// 检查底盘命令是否包含无效数值。 @@ -118,6 +157,7 @@ namespace MyParking.Shared /// public string LastFailureReason => _chassis.LastMotionDecomposeFailureReason; + #endregion /// @@ -125,10 +165,45 @@ namespace MyParking.Shared /// public void ResetToBodyFrame() { + ActivateMotionFrame(0.0); + } + + /// + /// 激活指定运动方向对应的SendMotion坐标系。 + /// 0表示真实车头，正90度表示将车体左侧作为虚拟车头。 + /// + public void ActivateMotionFrame( + double motionDirectionRadians) + { + ValidateFinite( + motionDirectionRadians, + nameof(motionDirectionRadians)); + + var biasDegrees = + (float)( + -FrameTransform2D.NormalizeAngle( + motionDirectionRadians) * + RadiansToDegrees); + var currentBias = + _chassis.GetOriginBias(); + + if (Math.Abs(currentBias.X) <= + BiasTolerance && + Math.Abs(currentBias.Y) <= + BiasTolerance && + Math.Abs( + NormalizeDegrees( + currentBias.Z - + biasDegrees)) <= + BiasTolerance) + { + return; + } + _chassis.SetOriginBias( x: 0.0f, y: 0.0f, - th: 0.0f); + th: biasDegrees); } public MultiWheelChassisAdapter(MultiWheelChassis chassis, int vehicleId) { @@ -154,6 +229,7 @@ namespace MyParking.Shared // 保证SendXYThSpeed直接使用真实车体坐标系。 var maximumWheelRadiusMillimeters = 0.0; var maximumLongitudinalOffsetMillimeters = 0.0; + var maximumLateralOffsetMillimeters = 0.0; foreach (var wheel in wheels) { maximumWheelRadiusMillimeters = Math.Max( @@ -163,15 +239,22 @@ namespace MyParking.Shared maximumLongitudinalOffsetMillimeters = Math.Max( maximumLongitudinalOffsetMillimeters, Math.Abs(wheel.PhysicalPosition.X)); + + maximumLateralOffsetMillimeters = Math.Max( + maximumLateralOffsetMillimeters, + Math.Abs(wheel.PhysicalPosition.Y)); } MaximumWheelRadiusMeters = maximumWheelRadiusMillimeters / 1000.0; HalfWheelBaseMeters = maximumLongitudinalOffsetMillimeters / 1000.0; + HalfTrackWidthMeters = + maximumLateralOffsetMillimeters / 1000.0; if (MaximumWheelRadiusMeters <= 0.0 || - HalfWheelBaseMeters <= 0.0) + HalfWheelBaseMeters <= 0.0 || + HalfTrackWidthMeters <= 0.0) { throw new InvalidOperationException( "Wheel positions cannot produce valid chassis dimensions."); @@ -220,6 +303,145 @@ namespace MyParking.Shared } return success; } + + /// + /// 发送单车原地自转命令。 + /// 适配层使用rad/s，底层SendRotateMotion使用deg/s。 + /// + public bool SendRotateMotion( + double omegaRadiansPerSecond, + TimeSpan? interval = null) + { + ValidateFinite( + omegaRadiansPerSecond, + nameof(omegaRadiansPerSecond)); + EnsureBodyFrameIsActive(); + + var success = _chassis.SendRotateMotion( + (float)( + omegaRadiansPerSecond * + RadiansToDegrees), + interval); + + if (!success) + { + _chassis.PredefinedDriveStop(); + } + + return success; + } + + /// + /// 将指定运动方向上的虚拟阿克曼命令转换为车体二维速度。 + /// 运动方向0表示车头，正90度表示车体左侧； + /// 转向角为正时向该虚拟运动方向的左侧转弯。 + /// + public bool SendVirtualAckermann( + double motionDirectionRadians, + double speedMetersPerSecond, + double steeringRadians, + double virtualHalfWheelBaseMeters, + TimeSpan? interval = null) + { + ValidateFinite( + motionDirectionRadians, + nameof(motionDirectionRadians)); + ValidateFinite( + speedMetersPerSecond, + nameof(speedMetersPerSecond)); + ValidateFinite( + steeringRadians, + nameof(steeringRadians)); + ValidateFinite( + virtualHalfWheelBaseMeters, + nameof(virtualHalfWheelBaseMeters)); + + if (virtualHalfWheelBaseMeters <= 0.0) + { + throw new ArgumentOutOfRangeException( + nameof(virtualHalfWheelBaseMeters), + "虚拟阿克曼半轴距必须是正数。"); + } + + var steeringCosine = + Math.Cos(steeringRadians); + if (Math.Abs(steeringCosine) < 1e-6) + { + throw new ArgumentOutOfRangeException( + nameof(steeringRadians), + "虚拟阿克曼转向角不能达到正负90度。"); + } + + var vxMetersPerSecond = + speedMetersPerSecond * + Math.Cos(motionDirectionRadians); + var vyMetersPerSecond = + speedMetersPerSecond * + Math.Sin(motionDirectionRadians); + var omegaRadiansPerSecond = + speedMetersPerSecond * + Math.Tan(steeringRadians) / + virtualHalfWheelBaseMeters; + + return Send( + new ChassisCommand( + VehicleId, + new Twist2D( + vxMetersPerSecond, + vyMetersPerSecond, + omegaRadiansPerSecond)), + interval); + } + + /// + /// 在已经激活的运动坐标系中使用SendMotion执行虚拟阿克曼运动。 + /// 转向角均相对该运动坐标系表达；正90度运动系对应车体左侧蟹行。 + /// + public bool SendVirtualAckermannMotion( + double motionDirectionRadians, + double speedMetersPerSecond, + double steeringRadians, + TimeSpan? interval = null) + { + ValidateFinite( + motionDirectionRadians, + nameof(motionDirectionRadians)); + ValidateFinite( + speedMetersPerSecond, + nameof(speedMetersPerSecond)); + ValidateFinite( + steeringRadians, + nameof(steeringRadians)); + EnsureMotionFrameIsActive( + motionDirectionRadians); + + if (Math.Abs(steeringRadians) >= + Math.PI / 2.0) + { + throw new ArgumentOutOfRangeException( + nameof(steeringRadians), + "虚拟阿克曼转向角必须位于正负90度以内。"); + } + + var steeringDegrees = + (float)( + steeringRadians * + RadiansToDegrees); + var success = + _chassis.SendMotion( + (float)speedMetersPerSecond, + steeringDegrees, + -steeringDegrees, + interval); + + if (!success) + { + _chassis.PredefinedDriveStop(); + } + + return success; + } + /// /// 按底盘减速度配置平滑停车，需要在控制周期中持续调用。 /// @@ -235,6 +457,14 @@ namespace MyParking.Shared _chassis.PredefinedDriveStop(); } + /// + /// 清零XYTh驱动速度，但保留已经准备好的自转舵角和轮速方向。 + /// + public void StopXYThDrivePreserveSteeringState() + { + _chassis.StopXYThDrivePreserveSteeringState(); + } + /// /// 停车并将所有舵轮转到指定的车体角度。 /// 只调整舵轮角度，不产生车辆线速度。 @@ -339,6 +569,40 @@ namespace MyParking.Shared } return success; } + + /// + /// 将已到位的自转舵角和轮速方向一次性交接给XYTh， + /// 防止普通SendXYThSpeed正式运动首帧重新初始化运动状态。 + /// + public bool AdoptPreparedSpinForXYTh( + double toleranceRadians = + 2.0 * Math.PI / 180.0) + { + if (double.IsNaN(toleranceRadians) || + double.IsInfinity(toleranceRadians) || + toleranceRadians < 0.0) + { + throw new ArgumentOutOfRangeException( + nameof(toleranceRadians), + "自转状态交接容差必须是非负有限值。"); + } + + EnsureBodyFrameIsActive(); + + var success = + _chassis + .AdoptPreparedRotateWheelsForXYTh( + (float)( + toleranceRadians * + RadiansToDegrees)); + + if (!success) + { + _chassis.PredefinedDriveStop(); + } + + return success; + } /// /// 所有舵轮是否已对齐到原地自转方向。 /// diff --git a/data_process/plots/20260728_165903_215_Stanley_Straight4m_Trial1_angular_command.png b/data_process/plots/20260728_165903_215_Stanley_Straight4m_Trial1_angular_command.png deleted file mode 100644 index a06a201..0000000 Binary files a/data_process/plots/20260728_165903_215_Stanley_Straight4m_Trial1_angular_command.png and /dev/null differ diff --git a/data_process/plots/20260728_165903_215_Stanley_Straight4m_Trial1_speed_response.png b/data_process/plots/20260728_165903_215_Stanley_Straight4m_Trial1_speed_response.png deleted file mode 100644 index d263a0f..0000000 Binary files a/data_process/plots/20260728_165903_215_Stanley_Straight4m_Trial1_speed_response.png and /dev/null differ diff --git a/data_process/plots/20260728_165903_215_Stanley_Straight4m_Trial1_tracking_errors.png b/data_process/plots/20260728_165903_215_Stanley_Straight4m_Trial1_tracking_errors.png deleted file mode 100644 index 9f3eaf1..0000000 Binary files a/data_process/plots/20260728_165903_215_Stanley_Straight4m_Trial1_tracking_errors.png and /dev/null differ diff --git a/data_process/plots/20260728_165903_215_Stanley_Straight4m_Trial1_trajectory_comparison.png b/data_process/plots/20260728_165903_215_Stanley_Straight4m_Trial1_trajectory_comparison.png deleted file mode 100644 index b65b591..0000000 Binary files a/data_process/plots/20260728_165903_215_Stanley_Straight4m_Trial1_trajectory_comparison.png and /dev/null differ diff --git a/data_process/plots/20260728_165946_321_InPlaceRotatePID_Rotate90_Trial1_angular_command.png b/data_process/plots/20260728_165946_321_InPlaceRotatePID_Rotate90_Trial1_angular_command.png deleted file mode 100644 index de66614..0000000 Binary files a/data_process/plots/20260728_165946_321_InPlaceRotatePID_Rotate90_Trial1_angular_command.png and /dev/null differ diff --git a/data_process/plots/20260728_165946_321_InPlaceRotatePID_Rotate90_Trial1_speed_response.png b/data_process/plots/20260728_165946_321_InPlaceRotatePID_Rotate90_Trial1_speed_response.png deleted file mode 100644 index ed93e8a..0000000 Binary files a/data_process/plots/20260728_165946_321_InPlaceRotatePID_Rotate90_Trial1_speed_response.png and /dev/null differ diff --git a/data_process/plots/20260728_165946_321_InPlaceRotatePID_Rotate90_Trial1_tracking_errors.png b/data_process/plots/20260728_165946_321_InPlaceRotatePID_Rotate90_Trial1_tracking_errors.png deleted file mode 100644 index 525d993..0000000 Binary files a/data_process/plots/20260728_165946_321_InPlaceRotatePID_Rotate90_Trial1_tracking_errors.png and /dev/null differ diff --git a/data_process/plots/20260728_165946_321_InPlaceRotatePID_Rotate90_Trial1_trajectory_comparison.png b/data_process/plots/20260728_165946_321_InPlaceRotatePID_Rotate90_Trial1_trajectory_comparison.png deleted file mode 100644 index db2a7ed..0000000 Binary files a/data_process/plots/20260728_165946_321_InPlaceRotatePID_Rotate90_Trial1_trajectory_comparison.png and /dev/null differ diff --git a/data_process/plots/20260728_170911_598_Stanley_Straight4m_Trial1_angular_command.png b/data_process/plots/20260728_170911_598_Stanley_Straight4m_Trial1_angular_command.png deleted file mode 100644 index b52884c..0000000 Binary files a/data_process/plots/20260728_170911_598_Stanley_Straight4m_Trial1_angular_command.png and /dev/null differ diff --git a/data_process/plots/20260728_170911_598_Stanley_Straight4m_Trial1_speed_response.png b/data_process/plots/20260728_170911_598_Stanley_Straight4m_Trial1_speed_response.png deleted file mode 100644 index eecb569..0000000 Binary files a/data_process/plots/20260728_170911_598_Stanley_Straight4m_Trial1_speed_response.png and /dev/null differ diff --git a/data_process/plots/20260728_170911_598_Stanley_Straight4m_Trial1_tracking_errors.png b/data_process/plots/20260728_170911_598_Stanley_Straight4m_Trial1_tracking_errors.png deleted file mode 100644 index f0aa2ed..0000000 Binary files a/data_process/plots/20260728_170911_598_Stanley_Straight4m_Trial1_tracking_errors.png and /dev/null differ diff --git a/data_process/plots/20260728_170911_598_Stanley_Straight4m_Trial1_trajectory_comparison.png b/data_process/plots/20260728_170911_598_Stanley_Straight4m_Trial1_trajectory_comparison.png deleted file mode 100644 index 1380815..0000000 Binary files a/data_process/plots/20260728_170911_598_Stanley_Straight4m_Trial1_trajectory_comparison.png and /dev/null differ diff --git a/data_process/plots/20260728_170949_530_InPlaceRotatePID_Rotate90_Trial1_angular_command.png b/data_process/plots/20260728_170949_530_InPlaceRotatePID_Rotate90_Trial1_angular_command.png deleted file mode 100644 index b1e8c9d..0000000 Binary files a/data_process/plots/20260728_170949_530_InPlaceRotatePID_Rotate90_Trial1_angular_command.png and /dev/null differ diff --git a/data_process/plots/20260728_170949_530_InPlaceRotatePID_Rotate90_Trial1_speed_response.png b/data_process/plots/20260728_170949_530_InPlaceRotatePID_Rotate90_Trial1_speed_response.png deleted file mode 100644 index c082944..0000000 Binary files a/data_process/plots/20260728_170949_530_InPlaceRotatePID_Rotate90_Trial1_speed_response.png and /dev/null differ diff --git a/data_process/plots/20260728_170949_530_InPlaceRotatePID_Rotate90_Trial1_tracking_errors.png b/data_process/plots/20260728_170949_530_InPlaceRotatePID_Rotate90_Trial1_tracking_errors.png deleted file mode 100644 index 1bd2151..0000000 Binary files a/data_process/plots/20260728_170949_530_InPlaceRotatePID_Rotate90_Trial1_tracking_errors.png and /dev/null differ diff --git a/data_process/plots/20260728_170949_530_InPlaceRotatePID_Rotate90_Trial1_trajectory_comparison.png b/data_process/plots/20260728_170949_530_InPlaceRotatePID_Rotate90_Trial1_trajectory_comparison.png deleted file mode 100644 index bc01718..0000000 Binary files a/data_process/plots/20260728_170949_530_InPlaceRotatePID_Rotate90_Trial1_trajectory_comparison.png and /dev/null differ diff --git a/data_process/plots/20260728_171031_498_GeometricController_LeftArc90_R2000mm_Trial1_angular_command.png b/data_process/plots/20260728_171031_498_GeometricController_LeftArc90_R2000mm_Trial1_angular_command.png deleted file mode 100644 index f8aaa57..0000000 Binary files a/data_process/plots/20260728_171031_498_GeometricController_LeftArc90_R2000mm_Trial1_angular_command.png and /dev/null differ diff --git a/data_process/plots/20260728_171031_498_GeometricController_LeftArc90_R2000mm_Trial1_speed_response.png b/data_process/plots/20260728_171031_498_GeometricController_LeftArc90_R2000mm_Trial1_speed_response.png deleted file mode 100644 index a237629..0000000 Binary files a/data_process/plots/20260728_171031_498_GeometricController_LeftArc90_R2000mm_Trial1_speed_response.png and /dev/null differ diff --git a/data_process/plots/20260728_171031_498_GeometricController_LeftArc90_R2000mm_Trial1_tracking_errors.png b/data_process/plots/20260728_171031_498_GeometricController_LeftArc90_R2000mm_Trial1_tracking_errors.png deleted file mode 100644 index f871b82..0000000 Binary files a/data_process/plots/20260728_171031_498_GeometricController_LeftArc90_R2000mm_Trial1_tracking_errors.png and /dev/null differ diff --git a/data_process/plots/20260728_171031_498_GeometricController_LeftArc90_R2000mm_Trial1_trajectory_comparison.png b/data_process/plots/20260728_171031_498_GeometricController_LeftArc90_R2000mm_Trial1_trajectory_comparison.png deleted file mode 100644 index 0c78a7e..0000000 Binary files a/data_process/plots/20260728_171031_498_GeometricController_LeftArc90_R2000mm_Trial1_trajectory_comparison.png and /dev/null differ diff --git a/data_process/plots/20260728_171239_653_GeometricController_LeftArc90_R2000mm_Trial1_angular_command.png b/data_process/plots/20260728_171239_653_GeometricController_LeftArc90_R2000mm_Trial1_angular_command.png deleted file mode 100644 index 61a5670..0000000 Binary files a/data_process/plots/20260728_171239_653_GeometricController_LeftArc90_R2000mm_Trial1_angular_command.png and /dev/null differ diff --git a/data_process/plots/20260728_171239_653_GeometricController_LeftArc90_R2000mm_Trial1_speed_response.png b/data_process/plots/20260728_171239_653_GeometricController_LeftArc90_R2000mm_Trial1_speed_response.png deleted file mode 100644 index 1ce47a7..0000000 Binary files a/data_process/plots/20260728_171239_653_GeometricController_LeftArc90_R2000mm_Trial1_speed_response.png and /dev/null differ diff --git a/data_process/plots/20260728_171239_653_GeometricController_LeftArc90_R2000mm_Trial1_tracking_errors.png b/data_process/plots/20260728_171239_653_GeometricController_LeftArc90_R2000mm_Trial1_tracking_errors.png deleted file mode 100644 index b45d7fc..0000000 Binary files a/data_process/plots/20260728_171239_653_GeometricController_LeftArc90_R2000mm_Trial1_tracking_errors.png and /dev/null differ diff --git a/data_process/plots/20260728_171239_653_GeometricController_LeftArc90_R2000mm_Trial1_trajectory_comparison.png b/data_process/plots/20260728_171239_653_GeometricController_LeftArc90_R2000mm_Trial1_trajectory_comparison.png deleted file mode 100644 index 896d2d2..0000000 Binary files a/data_process/plots/20260728_171239_653_GeometricController_LeftArc90_R2000mm_Trial1_trajectory_comparison.png and /dev/null differ diff --git a/data_process/plots/20260728_171435_157_Stanley_Straight4m_Trial1_angular_command.png b/data_process/plots/20260728_171435_157_Stanley_Straight4m_Trial1_angular_command.png deleted file mode 100644 index 739f583..0000000 Binary files a/data_process/plots/20260728_171435_157_Stanley_Straight4m_Trial1_angular_command.png and /dev/null differ diff --git a/data_process/plots/20260728_171435_157_Stanley_Straight4m_Trial1_speed_response.png b/data_process/plots/20260728_171435_157_Stanley_Straight4m_Trial1_speed_response.png deleted file mode 100644 index 588a459..0000000 Binary files a/data_process/plots/20260728_171435_157_Stanley_Straight4m_Trial1_speed_response.png and /dev/null differ diff --git a/data_process/plots/20260728_171435_157_Stanley_Straight4m_Trial1_tracking_errors.png b/data_process/plots/20260728_171435_157_Stanley_Straight4m_Trial1_tracking_errors.png deleted file mode 100644 index 64e79a9..0000000 Binary files a/data_process/plots/20260728_171435_157_Stanley_Straight4m_Trial1_tracking_errors.png and /dev/null differ diff --git a/data_process/plots/20260728_171435_157_Stanley_Straight4m_Trial1_trajectory_comparison.png b/data_process/plots/20260728_171435_157_Stanley_Straight4m_Trial1_trajectory_comparison.png deleted file mode 100644 index 92250e5..0000000 Binary files a/data_process/plots/20260728_171435_157_Stanley_Straight4m_Trial1_trajectory_comparison.png and /dev/null differ diff --git a/data_process/plots/20260728_171526_590_InPlaceRotatePID_Rotate90_Trial1_angular_command.png b/data_process/plots/20260728_171526_590_InPlaceRotatePID_Rotate90_Trial1_angular_command.png deleted file mode 100644 index 365b24d..0000000 Binary files a/data_process/plots/20260728_171526_590_InPlaceRotatePID_Rotate90_Trial1_angular_command.png and /dev/null differ diff --git a/data_process/plots/20260728_171526_590_InPlaceRotatePID_Rotate90_Trial1_speed_response.png b/data_process/plots/20260728_171526_590_InPlaceRotatePID_Rotate90_Trial1_speed_response.png deleted file mode 100644 index c2ab944..0000000 Binary files a/data_process/plots/20260728_171526_590_InPlaceRotatePID_Rotate90_Trial1_speed_response.png and /dev/null differ diff --git a/data_process/plots/20260728_171526_590_InPlaceRotatePID_Rotate90_Trial1_tracking_errors.png b/data_process/plots/20260728_171526_590_InPlaceRotatePID_Rotate90_Trial1_tracking_errors.png deleted file mode 100644 index ecac997..0000000 Binary files a/data_process/plots/20260728_171526_590_InPlaceRotatePID_Rotate90_Trial1_tracking_errors.png and /dev/null differ diff --git a/data_process/plots/20260728_171526_590_InPlaceRotatePID_Rotate90_Trial1_trajectory_comparison.png b/data_process/plots/20260728_171526_590_InPlaceRotatePID_Rotate90_Trial1_trajectory_comparison.png deleted file mode 100644 index 6b4a8b0..0000000 Binary files a/data_process/plots/20260728_171526_590_InPlaceRotatePID_Rotate90_Trial1_trajectory_comparison.png and /dev/null differ diff --git a/data_process/plots/20260728_171615_594_GeometricController_LeftArc90_R2000mm_Trial1_angular_command.png b/data_process/plots/20260728_171615_594_GeometricController_LeftArc90_R2000mm_Trial1_angular_command.png deleted file mode 100644 index bc96913..0000000 Binary files a/data_process/plots/20260728_171615_594_GeometricController_LeftArc90_R2000mm_Trial1_angular_command.png and /dev/null differ diff --git a/data_process/plots/20260728_171615_594_GeometricController_LeftArc90_R2000mm_Trial1_speed_response.png b/data_process/plots/20260728_171615_594_GeometricController_LeftArc90_R2000mm_Trial1_speed_response.png deleted file mode 100644 index 54a058c..0000000 Binary files a/data_process/plots/20260728_171615_594_GeometricController_LeftArc90_R2000mm_Trial1_speed_response.png and /dev/null differ diff --git a/data_process/plots/20260728_171615_594_GeometricController_LeftArc90_R2000mm_Trial1_tracking_errors.png b/data_process/plots/20260728_171615_594_GeometricController_LeftArc90_R2000mm_Trial1_tracking_errors.png deleted file mode 100644 index 24d8436..0000000 Binary files a/data_process/plots/20260728_171615_594_GeometricController_LeftArc90_R2000mm_Trial1_tracking_errors.png and /dev/null differ diff --git a/data_process/plots/20260728_171615_594_GeometricController_LeftArc90_R2000mm_Trial1_trajectory_comparison.png b/data_process/plots/20260728_171615_594_GeometricController_LeftArc90_R2000mm_Trial1_trajectory_comparison.png deleted file mode 100644 index 6ce53e6..0000000 Binary files a/data_process/plots/20260728_171615_594_GeometricController_LeftArc90_R2000mm_Trial1_trajectory_comparison.png and /dev/null differ diff --git a/data_process/plots/20260728_171807_436_Stanley_Straight4m_Trial1_angular_command.png b/data_process/plots/20260728_171807_436_Stanley_Straight4m_Trial1_angular_command.png deleted file mode 100644 index 0d5a832..0000000 Binary files a/data_process/plots/20260728_171807_436_Stanley_Straight4m_Trial1_angular_command.png and /dev/null differ diff --git a/data_process/plots/20260728_171807_436_Stanley_Straight4m_Trial1_speed_response.png b/data_process/plots/20260728_171807_436_Stanley_Straight4m_Trial1_speed_response.png deleted file mode 100644 index 9a072d7..0000000 Binary files a/data_process/plots/20260728_171807_436_Stanley_Straight4m_Trial1_speed_response.png and /dev/null differ diff --git a/data_process/plots/20260728_171807_436_Stanley_Straight4m_Trial1_tracking_errors.png b/data_process/plots/20260728_171807_436_Stanley_Straight4m_Trial1_tracking_errors.png deleted file mode 100644 index 727879c..0000000 Binary files a/data_process/plots/20260728_171807_436_Stanley_Straight4m_Trial1_tracking_errors.png and /dev/null differ diff --git a/data_process/plots/20260728_171807_436_Stanley_Straight4m_Trial1_trajectory_comparison.png b/data_process/plots/20260728_171807_436_Stanley_Straight4m_Trial1_trajectory_comparison.png deleted file mode 100644 index 58ce204..0000000 Binary files a/data_process/plots/20260728_171807_436_Stanley_Straight4m_Trial1_trajectory_comparison.png and /dev/null differ diff --git a/data_process/plots/20260728_171904_112_InPlaceRotatePID_Rotate90_Trial1_angular_command.png b/data_process/plots/20260728_171904_112_InPlaceRotatePID_Rotate90_Trial1_angular_command.png deleted file mode 100644 index bf4a6e0..0000000 Binary files a/data_process/plots/20260728_171904_112_InPlaceRotatePID_Rotate90_Trial1_angular_command.png and /dev/null differ diff --git a/data_process/plots/20260728_171904_112_InPlaceRotatePID_Rotate90_Trial1_speed_response.png b/data_process/plots/20260728_171904_112_InPlaceRotatePID_Rotate90_Trial1_speed_response.png deleted file mode 100644 index 0cf83cf..0000000 Binary files a/data_process/plots/20260728_171904_112_InPlaceRotatePID_Rotate90_Trial1_speed_response.png and /dev/null differ diff --git a/data_process/plots/20260728_171904_112_InPlaceRotatePID_Rotate90_Trial1_tracking_errors.png b/data_process/plots/20260728_171904_112_InPlaceRotatePID_Rotate90_Trial1_tracking_errors.png deleted file mode 100644 index 7e82bd0..0000000 Binary files a/data_process/plots/20260728_171904_112_InPlaceRotatePID_Rotate90_Trial1_tracking_errors.png and /dev/null differ diff --git a/data_process/plots/20260728_171904_112_InPlaceRotatePID_Rotate90_Trial1_trajectory_comparison.png b/data_process/plots/20260728_171904_112_InPlaceRotatePID_Rotate90_Trial1_trajectory_comparison.png deleted file mode 100644 index e717d5c..0000000 Binary files a/data_process/plots/20260728_171904_112_InPlaceRotatePID_Rotate90_Trial1_trajectory_comparison.png and /dev/null differ diff --git a/data_process/plots/20260728_171936_316_GeometricController_LeftArc90_R2000mm_Trial1_angular_command.png b/data_process/plots/20260728_171936_316_GeometricController_LeftArc90_R2000mm_Trial1_angular_command.png deleted file mode 100644 index e8dcbe6..0000000 Binary files a/data_process/plots/20260728_171936_316_GeometricController_LeftArc90_R2000mm_Trial1_angular_command.png and /dev/null differ diff --git a/data_process/plots/20260728_171936_316_GeometricController_LeftArc90_R2000mm_Trial1_speed_response.png b/data_process/plots/20260728_171936_316_GeometricController_LeftArc90_R2000mm_Trial1_speed_response.png deleted file mode 100644 index f7f5e40..0000000 Binary files a/data_process/plots/20260728_171936_316_GeometricController_LeftArc90_R2000mm_Trial1_speed_response.png and /dev/null differ diff --git a/data_process/plots/20260728_171936_316_GeometricController_LeftArc90_R2000mm_Trial1_tracking_errors.png b/data_process/plots/20260728_171936_316_GeometricController_LeftArc90_R2000mm_Trial1_tracking_errors.png deleted file mode 100644 index 0534ebc..0000000 Binary files a/data_process/plots/20260728_171936_316_GeometricController_LeftArc90_R2000mm_Trial1_tracking_errors.png and /dev/null differ diff --git a/data_process/plots/20260728_171936_316_GeometricController_LeftArc90_R2000mm_Trial1_trajectory_comparison.png b/data_process/plots/20260728_171936_316_GeometricController_LeftArc90_R2000mm_Trial1_trajectory_comparison.png deleted file mode 100644 index 4ebd46e..0000000 Binary files a/data_process/plots/20260728_171936_316_GeometricController_LeftArc90_R2000mm_Trial1_trajectory_comparison.png and /dev/null differ diff --git a/data_process/plots/20260728_172106_934_Stanley_Straight4m_Trial1_angular_command.png b/data_process/plots/20260728_172106_934_Stanley_Straight4m_Trial1_angular_command.png deleted file mode 100644 index 4c45dcd..0000000 Binary files a/data_process/plots/20260728_172106_934_Stanley_Straight4m_Trial1_angular_command.png and /dev/null differ diff --git a/data_process/plots/20260728_172106_934_Stanley_Straight4m_Trial1_speed_response.png b/data_process/plots/20260728_172106_934_Stanley_Straight4m_Trial1_speed_response.png deleted file mode 100644 index b9140f5..0000000 Binary files a/data_process/plots/20260728_172106_934_Stanley_Straight4m_Trial1_speed_response.png and /dev/null differ diff --git a/data_process/plots/20260728_172106_934_Stanley_Straight4m_Trial1_tracking_errors.png b/data_process/plots/20260728_172106_934_Stanley_Straight4m_Trial1_tracking_errors.png deleted file mode 100644 index 7da4f69..0000000 Binary files a/data_process/plots/20260728_172106_934_Stanley_Straight4m_Trial1_tracking_errors.png and /dev/null differ diff --git a/data_process/plots/20260728_172106_934_Stanley_Straight4m_Trial1_trajectory_comparison.png b/data_process/plots/20260728_172106_934_Stanley_Straight4m_Trial1_trajectory_comparison.png deleted file mode 100644 index 085f617..0000000 Binary files a/data_process/plots/20260728_172106_934_Stanley_Straight4m_Trial1_trajectory_comparison.png and /dev/null differ diff --git a/data_process/plots/20260728_172156_145_InPlaceRotatePID_Rotate90_Trial1_angular_command.png b/data_process/plots/20260728_172156_145_InPlaceRotatePID_Rotate90_Trial1_angular_command.png deleted file mode 100644 index 733baa6..0000000 Binary files a/data_process/plots/20260728_172156_145_InPlaceRotatePID_Rotate90_Trial1_angular_command.png and /dev/null differ diff --git a/data_process/plots/20260728_172156_145_InPlaceRotatePID_Rotate90_Trial1_speed_response.png b/data_process/plots/20260728_172156_145_InPlaceRotatePID_Rotate90_Trial1_speed_response.png deleted file mode 100644 index d1a7158..0000000 Binary files a/data_process/plots/20260728_172156_145_InPlaceRotatePID_Rotate90_Trial1_speed_response.png and /dev/null differ diff --git a/data_process/plots/20260728_172156_145_InPlaceRotatePID_Rotate90_Trial1_tracking_errors.png b/data_process/plots/20260728_172156_145_InPlaceRotatePID_Rotate90_Trial1_tracking_errors.png deleted file mode 100644 index d75e52a..0000000 Binary files a/data_process/plots/20260728_172156_145_InPlaceRotatePID_Rotate90_Trial1_tracking_errors.png and /dev/null differ diff --git a/data_process/plots/20260728_172156_145_InPlaceRotatePID_Rotate90_Trial1_trajectory_comparison.png b/data_process/plots/20260728_172156_145_InPlaceRotatePID_Rotate90_Trial1_trajectory_comparison.png deleted file mode 100644 index 7a65348..0000000 Binary files a/data_process/plots/20260728_172156_145_InPlaceRotatePID_Rotate90_Trial1_trajectory_comparison.png and /dev/null differ diff --git a/data_process/plots/20260728_172227_887_GeometricController_LeftArc90_R2000mm_Trial1_angular_command.png b/data_process/plots/20260728_172227_887_GeometricController_LeftArc90_R2000mm_Trial1_angular_command.png deleted file mode 100644 index 939ee77..0000000 Binary files a/data_process/plots/20260728_172227_887_GeometricController_LeftArc90_R2000mm_Trial1_angular_command.png and /dev/null differ diff --git a/data_process/plots/20260728_172227_887_GeometricController_LeftArc90_R2000mm_Trial1_speed_response.png b/data_process/plots/20260728_172227_887_GeometricController_LeftArc90_R2000mm_Trial1_speed_response.png deleted file mode 100644 index 1fe7888..0000000 Binary files a/data_process/plots/20260728_172227_887_GeometricController_LeftArc90_R2000mm_Trial1_speed_response.png and /dev/null differ diff --git a/data_process/plots/20260728_172227_887_GeometricController_LeftArc90_R2000mm_Trial1_tracking_errors.png b/data_process/plots/20260728_172227_887_GeometricController_LeftArc90_R2000mm_Trial1_tracking_errors.png deleted file mode 100644 index 184ff9f..0000000 Binary files a/data_process/plots/20260728_172227_887_GeometricController_LeftArc90_R2000mm_Trial1_tracking_errors.png and /dev/null differ diff --git a/data_process/plots/20260728_172227_887_GeometricController_LeftArc90_R2000mm_Trial1_trajectory_comparison.png b/data_process/plots/20260728_172227_887_GeometricController_LeftArc90_R2000mm_Trial1_trajectory_comparison.png deleted file mode 100644 index 24841d0..0000000 Binary files a/data_process/plots/20260728_172227_887_GeometricController_LeftArc90_R2000mm_Trial1_trajectory_comparison.png and /dev/null differ diff --git a/ref/CommonUsage.dll b/ref/CommonUsage.dll index 209c020..37c2382 100644 Binary files a/ref/CommonUsage.dll and b/ref/CommonUsage.dll differ diff --git a/电机记录.txt b/电机记录.txt new file mode 100644 index 0000000..5baa90f --- /dev/null +++ b/电机记录.txt @@ -0,0 +1,119 @@ +我检查了这组日志。结论是：上层目标速度和M层计算基本正常，CAN通信也比较稳定；真正明显的波动发生在驱动器返回的实际电机速度，尤其是右前轮组。 + +数据文件： + +- [CAN原始反馈]() +- [周期快照]() + +主要结果： + +| 项目 | 结果 | +|---|---:| +| 测试总时长 | 27.5s | +| 有效运动时间 | 约23.1s | +| 稳态目标速度 | 0.30045m/s | +| CAN反馈频率 | 每个电机约20Hz | +| CAN记录帧数 | 4413帧 | +| 记录器丢帧 | 未发现 | +| 整车平均实际速度 | 0.29988m/s | +| 整车平均速度标准差 | 0.01538m/s，约5.1% | + +软件下发没有跳变 + +8个原始目标速度在稳态完全一致： + +```text +CmdLFL～CmdRRR = 0.30045m/s +标准差 = 0 +``` + +启动阶段也不是突然给到0.3，而是大约1秒内平滑上升： + +```text +0 → 0.03 → 0.06 → …… → 0.30045m/s +``` + +因此不是遥控器、`SendMotion`或者速度加减速逻辑产生的目标跳变。 + +舵轮PID修正也很小 + +`PidLFL～PidRRR` 的范围大约是： + +```text +0.2991～0.3018m/s +``` + +修正量只有约 `±0.001m/s`。所以不是 [MotorRoutine.cs]() 中的舵角差速PID把轮速改成了0.2～0.4。 + +实际CAN反馈波动明显 + +稳态阶段8个电机反馈范围如下： + +| 电机 | 均值 m/s | 标准差 | 最小～最大 m/s | +|---|---:|---:|---:| +| LFL | 0.3010 | 0.0332 | 0.229～0.377 | +| LFR | 0.2990 | 0.0340 | 0.217～0.369 | +| LRL | 0.2996 | 0.0261 | 0.236～0.387 | +| LRR | 0.3001 | 0.0304 | 0.226～0.374 | +| RFL | 0.2992 | 0.0380 | 0.211～0.387 | +| RFR | 0.3021 | 0.0480 | 0.212～0.397 | +| RRL | 0.3013 | 0.0274 | 0.231～0.383 | +| RRR | 0.2982 | 0.0321 | 0.222～0.388 | + +均值都接近0.3，说明： + +- RPM解码和单位换算没有明显错误。 +- 减速比10.5、轮径85mm至少在平均值上是匹配的。 +- 正负号处理正确，没有运行中正负翻转。 + +但瞬时波动确实比较大，右前右电机 `RFR` 最明显。 + +舵角不是主要原因 + +直线阶段： + +```text +四个目标舵角始终为 0° +实际舵角基本在 ±0.4°以内 +``` + +没有出现舵角从正角度跳到负等价角，也没有发生接近180°的方向重选。因此这次轮速波动不是舵轮方向正负切换造成的。 + +CAN通信整体正常 + +8个驱动器基本都是： + +```text +反馈频率 ≈ 20Hz +通常最大间隔 ≈ 63ms +``` + +只有 `LFR` 出现过一次约105ms的间隔，像是偶发少一帧，但不足以解释整个测试期间持续的轮速波动。 + +需要注意的问题 + +同一舵轮内部两个电机的实际速度差，瞬时可达到约： + +```text +0.075～0.10m/s +``` + +但软件下发给两个电机的差值只有约 `0.001m/s`。这说明较大的左右电机速度差不是M层主动要求的。 + +可能来源依次是： + +1. 驱动器自身速度闭环参数或速度反馈滤波不足。 +2. 编码器瞬时速度估计噪声。 +3. 同一舵轮两个驱动电机负载不均。 +4. 机械间隙、轮胎形变或地面摩擦变化。 +5. 驱动器内部电流限幅或母线电压波动。 + +另外，`snapshot.csv` 实际只有约12.8Hz，而不是期望的50Hz。原始CAN仍有20Hz，所以本次诊断结论有效；但说明 `MCURoutine.Operation()` 中的同步串口、IO和多次CAN调用占用了较长时间。以后做快速轨迹控制时需要单独处理这个周期问题。 + +下一步最有价值的测试是： + +- 把驱动轮架空，同样下发0.3m/s并记录一次。 +- 落地空载，再记录一次。 +- 分别测试0.1、0.2、0.3m/s，每档保持10秒。 + +如果架空后仍然在0.2～0.4之间波动，重点检查驱动器速度环和编码器速度反馈；如果架空稳定、落地波动，则重点检查机械负载、轮胎和同一舵轮双电机同步。 \ No newline at end of file