diff --git a/src/main/java/edu/wpi/first/pathweaver/MainController.java b/src/main/java/edu/wpi/first/pathweaver/MainController.java index f8ed60a5..7ccd79d3 100644 --- a/src/main/java/edu/wpi/first/pathweaver/MainController.java +++ b/src/main/java/edu/wpi/first/pathweaver/MainController.java @@ -344,13 +344,20 @@ private void buildPaths() { java.nio.file.Path pathNameFile = output.resolve(path.getPathNameNoExtension()); - if(!path.getSpline().writeToFile(pathNameFile)) { + if(!path.getSpline().writePathToFile(path)) { Alert alert = new Alert(Alert.AlertType.WARNING); FxUtils.applyDarkMode(alert); alert.setTitle("Path export failure!"); alert.setContentText("Could not export to: " + output.toAbsolutePath()); } + + if(!path.getSpline().writeToFile(pathNameFile)) { + Alert alert = new Alert(Alert.AlertType.WARNING); + alert.setTitle("Trajectory export failure!"); + alert.setContentText("Could not export to: " + output.toAbsolutePath()); + } } + Alert alert = new Alert(Alert.AlertType.INFORMATION); FxUtils.applyDarkMode(alert); alert.setTitle("Paths exported!"); diff --git a/src/main/java/edu/wpi/first/pathweaver/PathIOUtil.java b/src/main/java/edu/wpi/first/pathweaver/PathIOUtil.java index b89e0c67..72c8d5d2 100644 --- a/src/main/java/edu/wpi/first/pathweaver/PathIOUtil.java +++ b/src/main/java/edu/wpi/first/pathweaver/PathIOUtil.java @@ -24,7 +24,6 @@ public final class PathIOUtil { private PathIOUtil() { } - /** * Exports path object to csv file. * @@ -33,7 +32,7 @@ private PathIOUtil() { * * @return true if successful file write was preformed */ - public static boolean export(String fileLocation, Path path) { + public static boolean export(String fileLocation, Path path, double yoffset) { try ( BufferedWriter writer = Files.newBufferedWriter(Paths.get(fileLocation + path.getPathName())); @@ -42,7 +41,7 @@ public static boolean export(String fileLocation, Path path) { ) { for (Waypoint wp : path.getWaypoints()) { double xPos = wp.getX(); - double yPos = wp.getY(); + double yPos = wp.getY() + yoffset; double tangentX = wp.getTangentX(); double tangentY = wp.getTangentY(); String name = wp.getName(); diff --git a/src/main/java/edu/wpi/first/pathweaver/SaveManager.java b/src/main/java/edu/wpi/first/pathweaver/SaveManager.java index d8b4c8f0..f2782819 100644 --- a/src/main/java/edu/wpi/first/pathweaver/SaveManager.java +++ b/src/main/java/edu/wpi/first/pathweaver/SaveManager.java @@ -87,7 +87,7 @@ public void saveChange(Path path) { */ private void saveChange(Path path, boolean remove) { String pathDirectory = ProjectPreferences.getInstance().getDirectory() + "/Paths/"; - PathIOUtil.export(pathDirectory, path); + PathIOUtil.export(pathDirectory, path, 0d); if (remove) { paths.remove(path); } diff --git a/src/main/java/edu/wpi/first/pathweaver/spline/Spline.java b/src/main/java/edu/wpi/first/pathweaver/spline/Spline.java index d587d4ea..a5a50ced 100644 --- a/src/main/java/edu/wpi/first/pathweaver/spline/Spline.java +++ b/src/main/java/edu/wpi/first/pathweaver/spline/Spline.java @@ -1,10 +1,10 @@ package edu.wpi.first.pathweaver.spline; +import edu.wpi.first.pathweaver.path.Path; + import edu.wpi.first.pathweaver.global.CurrentSelections; import javafx.scene.Group; -import java.nio.file.Path; - /** * This interface represents a Spline - the function that describes the path * the robot will take when travelling across the field. This class is designed for use with @@ -62,5 +62,12 @@ public interface Spline { * @param path the path of the file to write to * @return whether the write succeeded */ - boolean writeToFile(Path path); + boolean writeToFile(java.nio.file.Path path); + + /** + * Export this path to a file for use in the robot. + * @param path the path of the file to write to + * @return whether the write succeeded + */ + boolean writePathToFile(Path path); } 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 index a1e1f248..9a00c57d 100644 --- a/src/main/java/edu/wpi/first/pathweaver/spline/wpilib/WpilibSpline.java +++ b/src/main/java/edu/wpi/first/pathweaver/spline/wpilib/WpilibSpline.java @@ -1,6 +1,7 @@ package edu.wpi.first.pathweaver.spline.wpilib; import edu.wpi.first.pathweaver.FxUtils; +import edu.wpi.first.pathweaver.PathIOUtil; import edu.wpi.first.pathweaver.PathUnits; import edu.wpi.first.pathweaver.ProjectPreferences; import edu.wpi.first.pathweaver.Waypoint; @@ -147,6 +148,31 @@ public boolean writeToFile(java.nio.file.Path path) { } } + @Override + public boolean writePathToFile(Path path) { + final AtomicBoolean okay = new AtomicBoolean(true); + TrajectoryGenerator.setErrorHandler((error, stacktrace) -> { + LOGGER.log(Level.WARNING, "Could not write Spline to file: " + error, stacktrace); + okay.set(false); + }); + + var prefs = ProjectPreferences.getInstance(); + var lengthUnit = prefs.getField().getUnit(); + + // This value has units of the length type. + double height = prefs.getField().getRealLength().getValue().doubleValue(); + + // 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); + } + + PathIOUtil.export(ProjectPreferences.getInstance().getValues().getOutputDir() + "/Waypoints/", path, height); + + return okay.get(); + } + private static QuinticHermiteSpline[] getQuinticSplinesFromWaypoints(Waypoint[] waypoints) { QuinticHermiteSpline[] splines = new QuinticHermiteSpline[waypoints.length - 1]; for (int i = 0; i < waypoints.length - 1; i++) {