测试后可正常执行

2026-08-14 16:19:15 +08:00
parent a13e345f83
commit 9fe8901c4c
25 changed files with 381 additions and 299 deletions
@@ -283,8 +283,6 @@ namespace MedullaAdapter
                Math.Sign(x);
            var steeringDegrees =
                -normalizedSteering * MaxManualTheta;
-            var frontTh = steeringDegrees;
-            var rearTh = -steeringDegrees;
            ManualMode = (int)mode;

            switch (mode)
@@ -292,16 +290,24 @@ namespace MedullaAdapter
                case ManualControlMode.Normal:
                    // 普通模式统一使用车体速度命令：
                    // X向前，行驶中连续改变角速度时舵轮边转、车辆边走。
-                    // SendBodyCommand(
-                    //     vx: speed,
-                    //     vy: 0.0,
-                    //     omegaRadiansPerSecond: omega,
-                    //     interval);
-                    Chassis.SendMotion(
+                    var normalOmegaRadiansPerSecond =
+                        speed *
+                        Math.Tan(
+                            AngleMath.DegreesToRadians(
+                                steeringDegrees)) /
+                        adapter.ControlPointRadiusMeters;
+                    if (!adapter.SendBodyTwist(
+                            new Twist2D(
                                speed,
-                                frontTh,
-                                rearTh,
-                                interval);
+                                0.0,
+                                normalOmegaRadiansPerSecond),
+                            interval))
+                    {
+                        adapter.StopImmediately();
+                        Console.WriteLine(
+                            "Normal SendMotion decomposition failed: " +
+                            adapter.LastFailureReason);
+                    }
                    break;
                case ManualControlMode.Crab:
                    // 舵轮机械范围为[-120°,120°]。
@@ -332,13 +338,15 @@ namespace MedullaAdapter

                    // 将车体左侧作为虚拟阿克曼车头，并在该运动坐标系中
                    // 复用与普通模式相同的SendMotion前后控制点解算。
-                    if (!adapter.SendVirtualAckermannMotion(
-                            motionDirectionRadians:
-                                Math.PI / 2.0,
-                            speedMetersPerSecond:
+                    var crabOmegaRadiansPerSecond =
+                        speed *
+                        Math.Tan(crabSteeringRadians) /
+                        adapter.ControlPointRadiusMeters;
+                    if (!adapter.SendBodyTwist(
+                            new Twist2D(
+                                0.0,
                                speed,
-                            steeringRadians:
-                                crabSteeringRadians,
+                                crabOmegaRadiansPerSecond),
                            interval))
                    {
                        adapter.StopImmediately();
@@ -380,13 +388,11 @@ namespace MedullaAdapter

                    // 普通安全版SendXYThSpeed只下发角速度，
                    // 四轮实际舵角未到位时不会开放驱动速度。
-                    if (!adapter.Send(
-                            new ChassisCommand(
-                                CarNum,
-                                new Twist2D(
-                                    0.0,
-                                    0.0,
-                                    spinOmegaRadiansPerSecond)),
+                    if (!adapter.SendBodyTwist(
+                            new Twist2D(
+                                0.0,
+                                0.0,
+                                spinOmegaRadiansPerSecond),
                            interval))
                    {
                        adapter.StopImmediately();
@@ -42,8 +42,8 @@
  </ItemGroup>

  <ItemGroup>
-  <Compile Include="..\Shared\Models\ChassisCommand.cs"
-           Link="Shared\Models\ChassisCommand.cs" />
+  <Compile Include="..\Shared\Models\MotionModels.cs"
+           Link="Shared\Models\MotionModels.cs" />

  <Compile Include="..\Shared\Mathematics\FrameTransform2D.cs"
           Link="Shared\Mathematics\FrameTransform2D.cs" />