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

MultiWheelC/Experiments/TrackingExperimentRecorder.cs

@@ -49,6 +49,8 @@ namespace MultiWheelC
         public double ControlHeadingErrorRadians;
         public double ControlDistanceToTrajectoryMeters;
         public double ControlRemainingDistanceMeters;
+        public double CurvaturePreviewDistanceMeters;
+        public double FeedforwardCurvaturePerMeter;
 
         // 并列保存Detour速度与轮速解算速度，避免StateBodyVx的数据来源产生歧义。
         public bool HasVelocityDiagnostics;
@@ -150,6 +152,8 @@ namespace MultiWheelC
         private double _controlHeadingErrorRadians;
         private double _controlDistanceToTrajectoryMeters;
         private double _controlRemainingDistanceMeters;
+        private double _curvaturePreviewDistanceMeters;
+        private double _feedforwardCurvaturePerMeter;
         private bool _hasVelocityDiagnostics;
         private double _detourEstimatedBodyVxMetersPerSecond;
         private bool _detourVelocityEstimateValid;
@@ -371,7 +375,9 @@ namespace MultiWheelC
             double lateralErrorMeters,
             double headingErrorRadians,
             double distanceToTrajectoryMeters,
-            double remainingDistanceMeters)
+            double remainingDistanceMeters,
+            double curvaturePreviewDistanceMeters,
+            double feedforwardCurvaturePerMeter)
         {
             lock (_stateSyncRoot)
             {
@@ -387,6 +393,10 @@ namespace MultiWheelC
                     distanceToTrajectoryMeters;
                 _controlRemainingDistanceMeters =
                     remainingDistanceMeters;
+                _curvaturePreviewDistanceMeters =
+                    curvaturePreviewDistanceMeters;
+                _feedforwardCurvaturePerMeter =
+                    feedforwardCurvaturePerMeter;
                 _hasControlReference = true;
             }
         }
@@ -480,6 +490,8 @@ namespace MultiWheelC
                 double controlHeadingErrorRadians;
                 double controlDistanceToTrajectoryMeters;
                 double controlRemainingDistanceMeters;
+                double curvaturePreviewDistanceMeters;
+                double feedforwardCurvaturePerMeter;
                 bool hasVelocityDiagnostics;
                 double detourEstimatedBodyVxMetersPerSecond;
                 bool detourVelocityEstimateValid;
@@ -540,6 +552,10 @@ namespace MultiWheelC
                         _controlDistanceToTrajectoryMeters;
                     controlRemainingDistanceMeters =
                         _controlRemainingDistanceMeters;
+                    curvaturePreviewDistanceMeters =
+                        _curvaturePreviewDistanceMeters;
+                    feedforwardCurvaturePerMeter =
+                        _feedforwardCurvaturePerMeter;
                     hasVelocityDiagnostics =
                         _hasVelocityDiagnostics;
                     detourEstimatedBodyVxMetersPerSecond =
@@ -587,6 +603,10 @@ namespace MultiWheelC
                         controlDistanceToTrajectoryMeters,
                     ControlRemainingDistanceMeters =
                         controlRemainingDistanceMeters,
+                    CurvaturePreviewDistanceMeters =
+                        curvaturePreviewDistanceMeters,
+                    FeedforwardCurvaturePerMeter =
+                        feedforwardCurvaturePerMeter,
                     HasVelocityDiagnostics =
                         hasVelocityDiagnostics,
                     DetourEstimatedBodyVxMetersPerSecond =
@@ -802,6 +822,8 @@ namespace MultiWheelC
                     "ControlHeadingErrorRadians," +
                     "ControlDistanceToTrajectoryMeters," +
                     "ControlRemainingDistanceMeters," +
+                    "CurvaturePreviewDistanceMeters," +
+                    "FeedforwardCurvaturePerMeter," +
                     "HasVelocityDiagnostics," +
                     "DetourEstimatedBodyVxMetersPerSecond," +
                     "DetourVelocityEstimateValid," +
@@ -907,6 +929,12 @@ namespace MultiWheelC
                         FormatOptional(
                             sample.HasControlReference,
                             sample.ControlRemainingDistanceMeters),
+                        FormatOptional(
+                            sample.HasControlReference,
+                            sample.CurvaturePreviewDistanceMeters),
+                        FormatOptional(
+                            sample.HasControlReference,
+                            sample.FeedforwardCurvaturePerMeter),
                         sample.HasVelocityDiagnostics
                             ? "1"
                             : "0",