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

Co-authored-by: Cursor <cursoragent@cursor.com>

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,9 +226,18 @@ namespace MultiWheelC
                         omega,
                         MaximumAngularSpeedRadiansPerSecond);
 
+                    var now = DateTime.Now;
+                    var interval = now - lastCommandTime;
+                    lastCommandTime = now;
+
+                    bool commandAccepted;
+                    Twist2D bodyTwist;
+                    if (CommandBackend ==
+                        ChassisCommandBackend.SendMotion)
+                    {
                         // 运动坐标系相对车体系旋转+90°：
                         // 运动系正向速度会转换成车体系+Y速度。
-                    var bodyTwist =
+                        bodyTwist =
                             FrameTransform2D
                                 .TransformTwistAtSamePoint(
                                     new Pose2D(
@@ -208,17 +249,143 @@ namespace MultiWheelC
                                         vyInMotion,
                                         omega));
 
-                    var now = DateTime.Now;
-                    var interval = now - lastCommandTime;
-                    lastCommandTime = now;
+                        // 将运动坐标系原点和前后几何控制点处的速度，
+                        // 转换为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);
-
-                    if (!adapter.Send(command, interval))
+                        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,
@@ -643,7 +844,12 @@ namespace MultiWheelC
                 FinishDistanceMillimeters < 0f ||
                 !IsFinite(FinishDistanceMillimeters) ||
                  TrackingTimeoutSeconds <= 0f ||
-                !IsFinite(TrackingTimeoutSeconds))
+                 !IsFinite(TrackingTimeoutSeconds) ||
+                 MaximumVirtualSteeringRadians <= 0.0 ||
+                 MaximumVirtualSteeringRadians >=
+                 Math.PI / 2.0 ||
+                 !IsFinite(
+                     MaximumVirtualSteeringRadians))
                  throw new ArgumentOutOfRangeException(
                      "蟹行轨迹测试参数无效。");
         }

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

ClumsyPilot/Movements.cs

@@ -436,33 +436,99 @@ namespace MultiWheelC
         // 将本周期PID角速度输出提供给实验记录器，单位deg/s。
         public Action<float> CommandAngularSpeedObserver;
 
+        // 自转前舵轮实际角度允许误差，单位deg。
+        public float WheelAlignmentToleranceDegrees = 2f;
+
+        // 自转舵轮连续保持到位的时间，单位s。
+        public float WheelAlignmentStableSeconds = 0.3f;
+
+        // 自转舵轮准备超时时间，单位s。
+        public float WheelAlignmentTimeoutSeconds = 10f;
+
         // 归一化到大约 [-180°, 180°]
         private static float RangeAngle(float theta)
         {
             return (float)(theta - Math.Round(theta / 360.0f) * 360);
         }
 
-        // 使用 PID 控制原地旋转到目标角度。
+        // 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。
         public override IEnumerable<bool> Get()
         {
+            if (Chassis == null)
+                throw new InvalidOperationException(
+                    "当前底盘不是MultiWheelChassis，无法执行原地自转。");
+
+            var adapter = new MultiWheelChassisAdapter(
+                Chassis,
+                PilotDefinition.Self.CarNum);
+
             try
             {
+                var alignmentStarted = DateTime.Now;
+                DateTime? alignedSince = null;
+                while (true)
+                {
+                    if (!adapter.PrepareSpin())
+                        throw new InvalidOperationException(
+                            "无法生成原地自转舵轮目标：" +
+                            adapter.LastFailureReason);
+
+                    if (adapter.AreSpinWheelsAligned)
+                    {
+                        if (alignedSince == null)
+                            alignedSince = DateTime.Now;
+
+                        if ((DateTime.Now - alignedSince.Value)
+                                .TotalSeconds >=
+                            WheelAlignmentStableSeconds)
+                            break;
+                    }
+                    else
+                    {
+                        alignedSince = null;
+                    }
+
+                    if ((DateTime.Now - alignmentStarted)
+                            .TotalSeconds >
+                        WheelAlignmentTimeoutSeconds)
+                        throw new TimeoutException(
+                            "原地自转舵轮在限定时间内未稳定到位。");
+
+                    yield return true;
+                }
+
                 var targetAngle = RangeAngle(AngleTarget);
                 var p = PidparamsRead();
                 thPid = new PIDController(ThetaReader, p.Kp);
                 thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
                     p.OutputUpperThreshold, p.SpeedAccPerSec);
+                var lastCommandTime = DateTime.Now;
 
                 while (true)
                 {
                     var s = thPid.GetResponse(targetAngle, true);
                     Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
                     CommandAngularSpeedObserver?.Invoke(s);
-                    if (!Chassis.SendRotateMotion(s))
+                    var now = DateTime.Now;
+                    var interval = now - lastCommandTime;
+                    lastCommandTime = now;
+
+                    // PID输出s为deg/s，Shared命令统一使用rad/s。
+                    // adapter.Send最终调用普通安全版SendXYThSpeed。
+                    var omegaRadiansPerSecond =
+                        s * (float)Math.PI / 180f;
+                    if (!adapter.Send(
+                            new ChassisCommand(
+                                PilotDefinition.Self.CarNum,
+                                new Twist2D(
+                                    0.0,
+                                    0.0,
+                                    omegaRadiansPerSecond)),
+                            interval))
                     {
                         throw new InvalidOperationException(
-                            "原地旋转底盘解算失败：" +
-                            Chassis.LastMotionDecomposeFailureReason);
+                            "安全XYTh原地旋转底盘解算失败：" +
+                            adapter.LastFailureReason);
                     }
                     if (thPid.IsArrived()) break;
                     yield return true;
@@ -473,7 +539,7 @@ namespace MultiWheelC
             finally
             {
                 CommandAngularSpeedObserver?.Invoke(0f);
-                Chassis.PredefinedDriveStop();
+                adapter.StopImmediately();
             }
         }
     }

ClumsyPilot/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")]

ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfoInputs.cache

@@ -1 +1 @@
-5d77794fa0720c6591db5b06ac60427413c18989a6f7b64420ccb07d122d85bc
+ad10c8964b21ff4923ef175481b5fc5b35f6275f95224074d0c26f781df45851

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

CommonUsage-MultiVehicleSync/commonusage/Chassis/MultiWheelChassis.cs

@@ -216,6 +216,48 @@ namespace CommonUsage.Chassis
             LastMoveTime = DateTime.Now;
         }
 
+        /// <summary>
+        /// 立即清零XYTh驱动轮速度，同时保留已经准备好的舵角目标和轮速方向。
+        /// 下一条非零命令仍需重新确认四轮实际舵角到位后才会开放驱动速度。
+        /// </summary>
+        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;
@@ -776,6 +818,75 @@ namespace CommonUsage.Chassis
             return true;
         }
 
+        /// <summary>
+        /// 将PrepareRotateWheels已经确认到位的舵角和轮速方向，
+        /// 原样交接给SendXYThSpeed，作为一段XYTh运动的初始状态。
+        /// 该方法不会调用ResetMotionState，因此不会重新选择等价舵角。
+        /// </summary>
+        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;
+        }
+
         /// <summary>
         /// 绕"已被 SetOriginBias 偏置到车队中心的原点"做原地旋转，可叠加一个车体系小幅纠偏旋量。
         /// </summary>
@@ -1001,7 +1112,7 @@ namespace CommonUsage.Chassis
             _rearBase = _wheelBases.Last();
         }
 
-		private List<SteerWheel> _steerWheels = new ();
+        private List<SteerWheel> _steerWheels = new();
         private float _frontBase, _rearBase;
         private List<float> _wheelBases;
         private List<float> _sendSpeeds;
@@ -1017,7 +1128,7 @@ namespace CommonUsage.Chassis
         // wheel will swing between 90 and -90.
         private List<int> _wheelDirs;
 
-		// call this before a new continuous motion happens
+        // call this before a new motion sequence happens
         private void ResetMotionState()
         {
             LastMoveTime = DateTime.Now;
@@ -1061,13 +1172,33 @@ namespace CommonUsage.Chassis
         /// 原地旋转时舵角误差对应的速度衰减宽度，单位为度。
         /// </summary>
         public float SteeringAlignmentSigmaDegrees { get; set; } = 8f;
+        /// <summary>
+        /// 单轮速度低于此值时认为其运动方向无意义，单位为m/s。
+        /// </summary>
+        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)
+        /// <summary>
+        /// 下发车体二维速度，并根据舵轮机械角度误差进行高斯降速。
+        /// 一段运动开始时必须先等待全部舵轮到位；运动过程中舵角误差越大，
+        /// 四轮驱动速度的统一缩放比例越小，适合作为默认安全接口。
+        /// vx、vy单位为m/s，vth单位为°/s。
+        /// </summary>
+        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)
             {
@@ -1092,6 +1223,7 @@ namespace CommonUsage.Chassis
             float[] sendSpeed = new float[_steerWheels.Count];
             var allWheelsAligned = true;
             var maximumAngleError = 0f;
+            var alignmentSpeedScale = 1f;
             const float initialAlignmentToleranceDegrees = 2f;
             var writeDiagnostics =
                 Debug &&
@@ -1102,24 +1234,43 @@ 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);
 
                 // 这里比较受机械限位约束的实际舵角，不使用圆周最短角。
@@ -1127,6 +1278,15 @@ namespace CommonUsage.Chassis
                     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;

CommonUsage-MultiVehicleSync/commonusage/CommonUsage.csproj

@@ -2,6 +2,8 @@
 
   <PropertyGroup>
     <TargetFramework>netstandard2.0</TargetFramework>
+    <AssemblyName>CommonUsage</AssemblyName>
+    <RootNamespace>CommonUsage</RootNamespace>
   </PropertyGroup>
 
   <PropertyGroup>

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,
+                    // 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();

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)
             {
@@ -266,6 +273,7 @@ namespace MedullaAdapter
                     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) =>

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
+{
+    /// <summary>
+    /// 在后台保存驱动器CAN速度事件和底盘周期快照，避免文件IO阻塞CAN回调。
+    /// </summary>
+    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<LogRecord> _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; } = "";
+
+        /// <summary>
+        /// 创建本次诊断的两个CSV文件并启动后台写入线程。
+        /// </summary>
+        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();
+            }
+        }
+
+        /// <summary>
+        /// 停止记录并等待队列中的诊断数据写入磁盘。
+        /// </summary>
+        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();
+            }
+        }
+
+        /// <summary>
+        /// 将一帧驱动器速度反馈加入内存队列，不在CAN回调中执行文件写入。
+        /// </summary>
+        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));
+        }
+
+        /// <summary>
+        /// 按最多50Hz记录一帧控制命令、PID输出、CAN反馈和舵角快照。
+        /// </summary>
+        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();
+        }
+    }
+}

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": [

MedullaAdapter/obj/MedullaAdapter.csproj.nuget.g.props

@@ -4,13 +4,13 @@
     <RestoreSuccess Condition=" '$(RestoreSuccess)' == '' ">True</RestoreSuccess>
     <RestoreTool Condition=" '$(RestoreTool)' == '' ">NuGet</RestoreTool>
     <ProjectAssetsFile Condition=" '$(ProjectAssetsFile)' == '' ">$(MSBuildThisFileDirectory)project.assets.json</ProjectAssetsFile>
-    <NuGetPackageRoot Condition=" '$(NuGetPackageRoot)' == '' ">$(UserProfile)\.nuget\packages\</NuGetPackageRoot>
-    <NuGetPackageFolders Condition=" '$(NuGetPackageFolders)' == '' ">C:\Users\admin\.nuget\packages\;C:\Program Files (x86)\Microsoft Visual Studio\Shared\NuGetPackages</NuGetPackageFolders>
+    <NuGetPackageRoot Condition=" '$(NuGetPackageRoot)' == '' ">C:\Users\CodexSandboxOffline\.nuget\packages\</NuGetPackageRoot>
+    <NuGetPackageFolders Condition=" '$(NuGetPackageFolders)' == '' ">C:\Users\CodexSandboxOffline\.nuget\packages\;C:\Program Files (x86)\Microsoft Visual Studio\Shared\NuGetPackages</NuGetPackageFolders>
     <NuGetProjectStyle Condition=" '$(NuGetProjectStyle)' == '' ">PackageReference</NuGetProjectStyle>
     <NuGetToolVersion Condition=" '$(NuGetToolVersion)' == '' ">6.14.3</NuGetToolVersion>
   </PropertyGroup>
   <ItemGroup Condition=" '$(ExcludeRestorePackageImports)' != 'true' ">
-    <SourceRoot Include="C:\Users\admin\.nuget\packages\" />
+    <SourceRoot Include="C:\Users\CodexSandboxOffline\.nuget\packages\" />
     <SourceRoot Include="C:\Program Files (x86)\Microsoft Visual Studio\Shared\NuGetPackages\" />
   </ItemGroup>
 </Project>

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": [

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": [],

Shared/MultiWheelChassisAdapter.cs

@@ -30,6 +30,12 @@ namespace MyParking.Shared
         /// </summary>
         public double HalfWheelBaseMeters { get; }
 
+        /// <summary>
+        /// 车体原点到最外侧舵轮中心的最大横向距离，单位为米。
+        /// 对称四舵轮底盘中，它也是蟹行虚拟阿克曼模型的半轴距。
+        /// </summary>
+        public double HalfTrackWidthMeters { get; }
+
         /// <summary>
         /// Width of the steering-alignment speed gate, in degrees.
         /// </summary>
@@ -58,19 +64,52 @@ namespace MyParking.Shared
         /// </summary>
         private void EnsureBodyFrameIsActive()
         {
+            EnsureMotionFrameIsActive(0.0);
+        }
+
+        /// <summary>
+        /// 检查旧底盘当前是否处于指定的运动坐标系。
+        /// motionDirectionRadians表示该运动系X轴在真实车体坐标系中的方向。
+        /// </summary>
+        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}°。");
+        }
+
+        /// <summary>
+        /// 将角度归一化到[-180°,180°]附近。
+        /// </summary>
+        private static float NormalizeDegrees(float degrees)
+        {
+            return (float)(
+                degrees -
+                Math.Round(degrees / 360.0) * 360.0);
         }
         /// <summary>
         /// 检查底盘命令是否包含无效数值。
@@ -118,6 +157,7 @@ namespace MyParking.Shared
         /// </summary>
         public string LastFailureReason =>
             _chassis.LastMotionDecomposeFailureReason;
+
         #endregion
 
         /// <summary>
@@ -125,10 +165,45 @@ namespace MyParking.Shared
         /// </summary>
         public void ResetToBodyFrame()
         {
+            ActivateMotionFrame(0.0);
+        }
+
+        /// <summary>
+        /// 激活指定运动方向对应的SendMotion坐标系。
+        /// 0表示真实车头，正90度表示将车体左侧作为虚拟车头。
+        /// </summary>
+        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;
         }
+
+        /// <summary>
+        /// 发送单车原地自转命令。
+        /// 适配层使用rad/s，底层SendRotateMotion使用deg/s。
+        /// </summary>
+        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;
+        }
+
+        /// <summary>
+        /// 将指定运动方向上的虚拟阿克曼命令转换为车体二维速度。
+        /// 运动方向0表示车头，正90度表示车体左侧；
+        /// 转向角为正时向该虚拟运动方向的左侧转弯。
+        /// </summary>
+        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);
+        }
+
+        /// <summary>
+        /// 在已经激活的运动坐标系中使用SendMotion执行虚拟阿克曼运动。
+        /// 转向角均相对该运动坐标系表达；正90度运动系对应车体左侧蟹行。
+        /// </summary>
+        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;
+        }
+
         /// <summary>
         /// 按底盘减速度配置平滑停车，需要在控制周期中持续调用。
         /// </summary>
@@ -235,6 +457,14 @@ namespace MyParking.Shared
             _chassis.PredefinedDriveStop();
         }
 
+        /// <summary>
+        /// 清零XYTh驱动速度，但保留已经准备好的自转舵角和轮速方向。
+        /// </summary>
+        public void StopXYThDrivePreserveSteeringState()
+        {
+            _chassis.StopXYThDrivePreserveSteeringState();
+        }
+
         /// <summary>
         /// 停车并将所有舵轮转到指定的车体角度。
         /// 只调整舵轮角度，不产生车辆线速度。
@@ -339,6 +569,40 @@ namespace MyParking.Shared
             }
             return success;
         }
+
+        /// <summary>
+        /// 将已到位的自转舵角和轮速方向一次性交接给XYTh，
+        /// 防止普通SendXYThSpeed正式运动首帧重新初始化运动状态。
+        /// </summary>
+        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;
+        }
         /// <summary>
         /// 所有舵轮是否已对齐到原地自转方向。
         /// </summary>

电机记录.txt

@@ -0,0 +1,119 @@
+我检查了这组日志。结论是：上层目标速度和M层计算基本正常，CAN通信也比较稳定；真正明显的波动发生在驱动器返回的实际电机速度，尤其是右前轮组。
+
+数据文件：
+
+- [CAN原始反馈](</D:/Users/Desktop/入职培训/停车机器人/MyParking/logs/wheel-speed/20260730_112709_961_Car1_can.csv>)
+- [周期快照](</D:/Users/Desktop/入职培训/停车机器人/MyParking/logs/wheel-speed/20260730_112709_961_Car1_snapshot.csv>)
+
+主要结果：
+
+| 项目 | 结果 |
+|---|---:|
+| 测试总时长 | 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](</D:/Users/Desktop/入职培训/停车机器人/MyParking/MedullaAdapter/MotorRoutine.cs:168>) 中的舵角差速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之间波动，重点检查驱动器速度环和编码器速度反馈；如果架空稳定、落地波动，则重点检查机械负载、轮胎和同一舵轮双电机同步。