重构横向控制器输出前后GCP目标转角并简化命令分配器

MultiWheelC/Movements/TrajectoryTrackingMovement.cs

@@ -55,6 +55,18 @@ namespace MultiWheelC
         /// </summary>
         public bool StanleyUsesActualSpeed = true;
 
+        /// <summary>
+        /// Stanley横向误差共同转角分量的最大绝对值，单位为rad。
+        /// </summary>
+        public double MaximumCrossTrackCorrectionRadians =
+            AngleMath.DegreesToRadians(10.0);
+
+        /// <summary>
+        /// Stanley航向误差差动转角分量的最大绝对值，单位为rad。
+        /// </summary>
+        public double MaximumHeadingCorrectionRadians =
+            AngleMath.DegreesToRadians(10.0);
+
         /// <summary>
         /// 纵向速度外环比例增益。
         /// </summary>
@@ -163,7 +175,9 @@ namespace MultiWheelC
                     StanleyCrossTrackGainPerSecond,
                     StanleyHeadingErrorGain,
                     StanleyMinimumSpeedMetersPerSecond,
-                    StanleyUsesActualSpeed);
+                    StanleyUsesActualSpeed,
+                    MaximumCrossTrackCorrectionRadians,
+                    MaximumHeadingCorrectionRadians);
             var longitudinalController =
                 new PidLongitudinalController(
                     LongitudinalKp,
@@ -173,8 +187,7 @@ namespace MultiWheelC
                     MaximumCommandSpeedMetersPerSecond,
                     LongitudinalSpeedErrorDeadbandMetersPerSecond);
             var gcpAllocator =
-                new AckermannGcpAllocator(
-                    controlPointRadiusMeters,
+                new GcpCommandAllocator(
                     MaximumGcpAngleRadians);
             var commandExecutor =
                 new GcpCommandExecutor(