增加差速电机前馈与控制器曲率预瞄

2026-08-12 11:19:01 +08:00
parent 3043febd91
commit 6c6149c8d5
24 changed files with 366 additions and 19 deletions
@@ -365,7 +365,10 @@ namespace MultiWheelC
                    projection.LateralErrorMeters,
                    projection.HeadingErrorRadians,
                    projection.DistanceToTrajectoryMeters,
-                    projection.RemainingDistanceMeters);
+                    projection.RemainingDistanceMeters,
+                    controller.LastCurvaturePreviewDistanceMeters ?? 0.0,
+                    controller.LastFeedforwardCurvaturePerMeter ??
+                    projection.ReferencePoint.CurvaturePerMeter);
            }

            var command = controller.LastCommand.Value;
@@ -728,7 +731,10 @@ namespace MultiWheelC
                    projection.LateralErrorMeters,
                    projection.HeadingErrorRadians,
                    projection.DistanceToTrajectoryMeters,
-                    projection.RemainingDistanceMeters);
+                    projection.RemainingDistanceMeters,
+                    controller.LastCurvaturePreviewDistanceMeters ?? 0.0,
+                    controller.LastFeedforwardCurvaturePerMeter ??
+                    projection.ReferencePoint.CurvaturePerMeter);
            }

            var command = controller.LastCommand.Value;