Fixes bug when non meter units were exported as meters in the traject…

…ory config. (#259)
wpilibsuite · Apr 21, 2021 · 765f50b · 765f50b
1 parent c7aecb6
commit 765f50b
Showing 1 changed file with 10 additions and 7 deletions.
diff --git a/src/main/java/edu/wpi/first/pathweaver/spline/wpilib/WpilibSpline.java b/src/main/java/edu/wpi/first/pathweaver/spline/wpilib/WpilibSpline.java
@@ -112,23 +112,26 @@ public boolean writeToFile(java.nio.file.Path path) {
         });
         try {
             var values = ProjectPreferences.getInstance().getValues();
-
-            TrajectoryConfig config = new TrajectoryConfig(values.getMaxVelocity(), values.getMaxAcceleration())
-                .setKinematics(new DifferentialDriveKinematics(values.getTrackWidth())).setReversed(waypoints.get(0).isReversed());
-            Trajectory traj = trajectoryFromWaypoints(waypoints, config);
-
             var prefs = ProjectPreferences.getInstance();
             var lengthUnit = prefs.getField().getUnit();
-
-            // This value has units of the length type.
             double height = prefs.getField().getRealLength().getValue().doubleValue();
+            var maxVelocity = values.getMaxVelocity();
+            var maxAcceleration = values.getMaxAcceleration();
+            var trackWidth = values.getTrackWidth();
 
             // If the export type is different (i.e. meters), then we have to convert it. Otherwise we are good.
             if (prefs.getValues().getExportUnit() == ProjectPreferences.ExportUnit.METER) {
                 UnitConverter converter = lengthUnit.getConverterTo(PathUnits.METER);
                 height = converter.convert(height);
+                maxVelocity = converter.convert(maxVelocity);
+                maxAcceleration = converter.convert(maxAcceleration);
+                trackWidth = converter.convert(trackWidth);
             }
 
+            TrajectoryConfig config = new TrajectoryConfig(maxVelocity, maxAcceleration)
+                .setKinematics(new DifferentialDriveKinematics(trackWidth)).setReversed(waypoints.get(0).isReversed());
+            Trajectory traj = trajectoryFromWaypoints(waypoints, config);
+
             for (int i = 0; i < traj.getStates().size(); ++i) {
                 var st = traj.getStates().get(i);
                 traj.getStates().set(i, new Trajectory.State(