实现蟹行轨迹跟踪测试并优化底盘XYTh与原地旋转舵角控制

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

ClumsyPilot/Movements.cs

@@ -251,7 +251,7 @@ namespace MultiWheelC
             finally
             {
                 task?.Stop();
-                chassis.SendXYThSpeed(0f, 0f, 0f);
+                chassis.PredefinedDriveStop();
             }
         }
 
@@ -458,7 +458,12 @@ namespace MultiWheelC
                     var s = thPid.GetResponse(targetAngle, true);
                     Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
                     CommandAngularSpeedObserver?.Invoke(s);
-                    Chassis.SendXYThSpeed(0, 0, s);
+                    if (!Chassis.SendRotateMotion(s))
+                    {
+                        throw new InvalidOperationException(
+                            "原地旋转底盘解算失败：" +
+                            Chassis.LastMotionDecomposeFailureReason);
+                    }
                     if (thPid.IsArrived()) break;
                     yield return true;
                 }
@@ -468,7 +473,7 @@ namespace MultiWheelC
             finally
             {
                 CommandAngularSpeedObserver?.Invoke(0f);
-                Chassis.SendXYThSpeed(0, 0, 0);
+                Chassis.PredefinedDriveStop();
             }
         }
     }