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

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

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();
             }
         }
     }