From eff54f5261ee9a8002e7d8fd182e66253997a1cc Mon Sep 17 00:00:00 2001 From: carbotaniuman <41451839+carbotaniuman@users.noreply.github.com> Date: Mon, 30 Dec 2019 22:17:51 -0500 Subject: [PATCH] Coord fix (#156) * Refactor the units system The new system is cleaner to use and more adaptable to alternative backends. Also cleaned up said code. * Add support for PathWeaver JSON * Fix coordinate issues, and export issue. Close #155. * Remove useless time-step parameter --- PathWeaver/pathweaver.json | 2 - build.gradle.kts | 9 +- .../pathweaver/CreateProjectController.java | 429 ++++++++--------- .../java/edu/wpi/first/pathweaver/Field.java | 273 +++++------ .../pathweaver/FieldDisplayController.java | 7 +- .../wpi/first/pathweaver/MainController.java | 23 +- .../edu/wpi/first/pathweaver/PathUnits.java | 305 ++++++++----- .../first/pathweaver/ProgramPreferences.java | 334 +++++++------- .../first/pathweaver/ProjectPreferences.java | 430 +++++++++--------- .../edu/wpi/first/pathweaver/Waypoint.java | 421 +++++++++-------- .../first/pathweaver/global/DragHandler.java | 8 +- .../pathweaver/path/wpilib/WpilibPath.java | 4 +- .../spline/wpilib/WpilibSpline.java | 53 ++- .../wpi/first/pathweaver/createProject.fxml | 90 ++-- .../pathweaver/extensions/GameLoaderTest.java | 59 ++- 15 files changed, 1266 insertions(+), 1181 deletions(-) diff --git a/PathWeaver/pathweaver.json b/PathWeaver/pathweaver.json index 01972a03..8fc76af9 100644 --- a/PathWeaver/pathweaver.json +++ b/PathWeaver/pathweaver.json @@ -1,8 +1,6 @@ { - "timeStep": 0.02, "maxVelocity": 10.0, "maxAcceleration": 60.0, - "maxJerk": 60.0, "wheelBase": 2.0, "gameName": "FIRST Power Up" } \ No newline at end of file diff --git a/build.gradle.kts b/build.gradle.kts index 816af293..ffc69515 100644 --- a/build.gradle.kts +++ b/build.gradle.kts @@ -124,8 +124,9 @@ dependencies { compile(group = "org.fxmisc.easybind", name = "easybind", version = "1.0.3") compile(group = "javax.measure", name = "unit-api", version = "1.0") - compile(group = "si.uom", name = "si-units", version = "0.9") - compile(group = "systems.uom", name = "systems-common", version = "0.8") + compile(group = "si.uom", name = "si-units", version = "2.0.1") + compile(group = "systems.uom", name = "systems-common", version = "2.0") + compile(group = "tech.units", name = "indriya", version = "2.0.1") compile(group = "com.google.code.gson", name = "gson", version = "2.8.5") compile(group = "org.apache.commons", name = "commons-csv", version = "1.5") @@ -134,6 +135,10 @@ dependencies { compile(group = "edu.wpi.first.wpiutil", name = "wpiutil-java", version = "2020.+") compile(group = "edu.wpi.first.wpilibj", name = "wpilibj-java", version = "2020.+") + compile(group = "com.fasterxml.jackson.core", name = "jackson-annotations", version = "2.10.0") + compile(group = "com.fasterxml.jackson.core", name = "jackson-core", version = "2.10.0") + compile(group = "com.fasterxml.jackson.core", name = "jackson-databind", version = "2.10.0") + fun junitJupiter(name: String, version: String = "5.5.0") = create(group = "org.junit.jupiter", name = name, version = version) diff --git a/src/main/java/edu/wpi/first/pathweaver/CreateProjectController.java b/src/main/java/edu/wpi/first/pathweaver/CreateProjectController.java index edf55350..0c98b82e 100644 --- a/src/main/java/edu/wpi/first/pathweaver/CreateProjectController.java +++ b/src/main/java/edu/wpi/first/pathweaver/CreateProjectController.java @@ -1,6 +1,5 @@ package edu.wpi.first.pathweaver; - import org.fxmisc.easybind.EasyBind; import java.io.File; @@ -25,237 +24,211 @@ import javafx.stage.DirectoryChooser; import javafx.util.StringConverter; import javafx.util.Duration; -import tec.units.ri.format.SimpleUnitFormat; +import tech.units.indriya.format.SimpleUnitFormat; import javax.measure.Unit; import javax.measure.quantity.Length; @SuppressWarnings("PMD") public class CreateProjectController { - @FXML - private Label title; - @FXML - private Button browse; - @FXML - private Button browseOutput; - @FXML - private Button create; - @FXML - private Button cancel; - @FXML - private VBox vBox; - @FXML - private TextField directory; - @FXML - private TextField outputDirectory; - @FXML - private TextField timeStep; - @FXML - private TextField maxVelocity; - @FXML - private TextField maxAcceleration; - @FXML - private TextField maxJerk; - @FXML - private TextField wheelBase; - @FXML - private ChoiceBox game; - @FXML - private ChoiceBox> length; - @FXML - private Label browseLabel; - @FXML - private Label timeLabel; - @FXML - private Label outputLabel; - @FXML - private Label velocityLabel; - @FXML - private Label accelerationLabel; - @FXML - private Label jerkLabel; - @FXML - private Label wheelBaseLabel; - @FXML - private Label timeUnits; - @FXML - private Label velocityUnits; - @FXML - private Label accelerationUnits; - @FXML - private Label jerkUnits; - @FXML - private Label wheelBaseUnits; - - private boolean editing = false; - - @FXML - - private void initialize() { - ObservableList numericFields = FXCollections.observableArrayList(timeStep, maxVelocity, - maxAcceleration, maxJerk, wheelBase); - ObservableList allFields = FXCollections.observableArrayList(numericFields); - allFields.add(directory); - - var directoryControls = List.of(browseLabel, directory, browse); - var outputControls = List.of(outputLabel, outputDirectory, browseOutput); - var timeControls = List.of(timeLabel, timeStep, timeUnits); - var velocityControls = List.of(velocityLabel, maxVelocity, velocityUnits); - var accelerationControls = List.of(accelerationLabel, maxAcceleration, accelerationUnits); - var jerkControls = List.of(jerkLabel, maxJerk, jerkUnits); - var wheelBaseControls = List.of(wheelBaseLabel, wheelBase, wheelBaseUnits); - - BooleanBinding bind = new SimpleBooleanProperty(true).not(); - for (TextField field : allFields) { - bind = bind.or(field.textProperty().isEmpty()); - } - bind = bind.or(game.valueProperty().isNull()); - create.disableProperty().bind(bind); - - - // Validate that numericFields contain decimal numbers - numericFields.forEach(textField -> textField.setTextFormatter(FxUtils.onlyPositiveDoubleText())); - - game.getItems().addAll(Game.getGames()); - game.getSelectionModel().selectFirst(); - game.converterProperty().setValue(new StringConverter<>() { - @Override - public String toString(Game object) { - return object.getName(); - } - - @Override - public Game fromString(String string) { - return Game.fromPrettyName(string); - } - }); - - length.getItems().addAll(PathUnits.LENGTHS); - length.getSelectionModel().selectFirst(); - length.setConverter(new StringConverter<>() { - @Override - public String toString(Unit object) { - return object.getName(); - } - - @Override - public Unit fromString(String string) { - throw new UnsupportedOperationException(); - } - }); - - var lengthUnit = EasyBind.monadic(length.getSelectionModel().selectedItemProperty()); - directoryControls.forEach(control -> control - .setTooltip(new Tooltip("The directory to store your project.\n" - + "It will be stored at a PathWeaver subdirectory of this location.\n" - + "It is recommended this be the root folder of your robot code\n" - + "to simplify version control and path deployment."))); - outputControls.forEach(control -> control - .setTooltip(new Tooltip("(Optional) The directory to output paths to.\n" - + "If it is the root folder of your FRC robot project,\nthe paths will automatically be copied to the " - + "robot at deploy time.\nDefault: will search relative to your project directory,\n" - + "attempting to find deploy folder."))); - timeControls.forEach(control -> control - .setTooltip(new Tooltip("Time delta between points"))); - velocityControls.forEach(control -> control - .setTooltip(new Tooltip("The maximum velocity your robot can travel."))); - velocityUnits.textProperty().bind(lengthUnit.map(PathUnits::velocity)); - accelerationControls.forEach(control -> control - .setTooltip(new Tooltip("The maximum capable acceleration of your robot."))); - accelerationUnits.textProperty().bind(lengthUnit.map(PathUnits::acceleration)); - jerkControls.forEach(control -> control - .setTooltip(new Tooltip("The maximum jerk to use when calculating motion profiles. " - + "This is user preference."))); - jerkUnits.textProperty().bind(lengthUnit.map(PathUnits::jerk)); - wheelBaseControls.forEach(control -> control - .setTooltip(new Tooltip("Distance between the left and right of the wheel base."))); - wheelBaseUnits.textProperty().bind(lengthUnit.map(SimpleUnitFormat.getInstance()::format)); - // Show longer text for an extended period of time - Stream.of(directoryControls, outputControls) - .flatMap(List::stream) - .forEach(control -> control.getTooltip().setShowDuration(Duration.seconds(10))); - Stream.of(directoryControls, outputControls, timeControls, velocityControls, accelerationControls, jerkControls, - wheelBaseControls) - .flatMap(List::stream) - .forEach(control -> control.getTooltip().setShowDelay(Duration.millis(150))); - - // We are editing a project - if (ProjectPreferences.getInstance() != null) { - setupEditProject(); - } - } - - @FXML - - private void createProject() { - String folderString = directory.getText().trim(); - // create a "PathWeaver" subdirectory if not editing an existing project - File directory = editing ? new File(folderString) : new File(folderString, "PathWeaver"); - directory.mkdir(); - String outputString = outputDirectory.getText(); - String outputPath = outputString; - boolean newOutput = !editing - || !Objects.equals(outputString, ProjectPreferences.getInstance().getValues().getOutputDir()); - if (outputString != null && !outputString.isEmpty() && newOutput) { - // Find the relative path for the output directory to the project directory, using / file separators - outputPath = directory.toPath().relativize(new File(outputString.trim()).toPath()).toString() - .replace("\\", "/"); - } - ProgramPreferences.getInstance().addProject(directory.getAbsolutePath()); - String lengthUnit = length.getValue().getName(); - double timeDelta = Double.parseDouble(timeStep.getText()); - double velocityMax = Double.parseDouble(maxVelocity.getText()); - double accelerationMax = Double.parseDouble(maxAcceleration.getText()); - double jerkMax = Double.parseDouble(maxJerk.getText()); - double wheelBaseDistance = Double.parseDouble(wheelBase.getText()); - ProjectPreferences.Values values = new ProjectPreferences.Values(lengthUnit, timeDelta, velocityMax, - accelerationMax, jerkMax, wheelBaseDistance, game.getValue().getName(), outputPath); - ProjectPreferences prefs = ProjectPreferences.getInstance(directory.getAbsolutePath()); - prefs.setValues(values); - editing = false; - FxUtils.loadMainScreen(vBox.getScene(), getClass()); - } - - @FXML - private void browseDirectory() { - browse(directory); - } - - @FXML - private void browseOutput() { - browse(outputDirectory); - } - - private void browse(TextField location) { - DirectoryChooser chooser = new DirectoryChooser(); - File selectedDirectory = chooser.showDialog(vBox.getScene().getWindow()); - if (selectedDirectory != null) { - location.setText(selectedDirectory.getPath()); - location.positionCaret(selectedDirectory.getPath().length()); - } - } - - @FXML - private void cancel() throws IOException { - Pane root = FXMLLoader.load(getClass().getResource("welcomeScreen.fxml")); - vBox.getScene().setRoot(root); - } - - private void setupEditProject() { - ProjectPreferences.Values values = ProjectPreferences.getInstance().getValues(); - directory.setText(ProjectPreferences.getInstance().getDirectory()); - outputDirectory.setText(ProjectPreferences.getInstance().getValues().getOutputDir()); - create.setText("Edit Project"); - title.setText("Edit Project"); - browse.setVisible(false); - cancel.setVisible(false); - game.setValue(Game.fromPrettyName(values.getGameName())); - length.setValue(values.getLengthUnit()); - timeStep.setText(String.valueOf(values.getTimeStep())); - maxVelocity.setText(String.valueOf(values.getMaxVelocity())); - maxAcceleration.setText(String.valueOf(values.getMaxAcceleration())); - maxJerk.setText(String.valueOf(values.getMaxJerk())); - wheelBase.setText(String.valueOf(values.getWheelBase())); - editing = true; - } + @FXML + private Label title; + @FXML + private Button browse; + @FXML + private Button browseOutput; + @FXML + private Button create; + @FXML + private Button cancel; + @FXML + private VBox vBox; + @FXML + private TextField directory; + @FXML + private TextField outputDirectory; + @FXML + private TextField maxVelocity; + @FXML + private TextField maxAcceleration; + @FXML + private TextField wheelBase; + @FXML + private ChoiceBox game; + @FXML + private ChoiceBox> length; + @FXML + private Label browseLabel; + @FXML + private Label outputLabel; + @FXML + private Label velocityLabel; + @FXML + private Label accelerationLabel; + @FXML + private Label wheelBaseLabel; + @FXML + private Label velocityUnits; + @FXML + private Label accelerationUnits; + @FXML + private Label wheelBaseUnits; + + private boolean editing = false; + + @FXML + + private void initialize() { + ObservableList numericFields = FXCollections.observableArrayList(maxVelocity, + maxAcceleration, wheelBase); + ObservableList allFields = FXCollections.observableArrayList(numericFields); + allFields.add(directory); + + var directoryControls = List.of(browseLabel, directory, browse); + var outputControls = List.of(outputLabel, outputDirectory, browseOutput); + var velocityControls = List.of(velocityLabel, maxVelocity, velocityUnits); + var accelerationControls = List.of(accelerationLabel, maxAcceleration, accelerationUnits); + var wheelBaseControls = List.of(wheelBaseLabel, wheelBase, wheelBaseUnits); + + BooleanBinding bind = new SimpleBooleanProperty(true).not(); + for (TextField field : allFields) { + bind = bind.or(field.textProperty().isEmpty()); + } + bind = bind.or(game.valueProperty().isNull()); + create.disableProperty().bind(bind); + + // Validate that numericFields contain decimal numbers + numericFields.forEach(textField -> textField.setTextFormatter(FxUtils.onlyPositiveDoubleText())); + + game.getItems().addAll(Game.getGames()); + game.getSelectionModel().selectFirst(); + game.converterProperty().setValue(new StringConverter<>() { + @Override + public String toString(Game object) { + return object.getName(); + } + + @Override + public Game fromString(String string) { + return Game.fromPrettyName(string); + } + }); + + length.getItems().addAll(PathUnits.LENGTHS); + length.getSelectionModel().selectFirst(); + length.setConverter(new StringConverter<>() { + @Override + public String toString(Unit object) { + return object.getName(); + } + + @Override + public Unit fromString(String string) { + throw new UnsupportedOperationException(); + } + }); + + var lengthUnit = EasyBind.monadic(length.getSelectionModel().selectedItemProperty()); + directoryControls.forEach(control -> control.setTooltip(new Tooltip("The directory to store your project.\n" + + "It will be stored at a PathWeaver subdirectory of this location.\n" + + "It is recommended this be the root folder of your robot code\n" + + "to simplify version control and path deployment."))); + outputControls + .forEach(control -> control.setTooltip(new Tooltip("(Optional) The directory to output paths to.\n" + + "If it is the root folder of your FRC robot project,\nthe paths will automatically be copied to the " + + "robot at deploy time.\nDefault: will search relative to your project directory,\n" + + "attempting to find deploy folder."))); + velocityControls + .forEach(control -> control.setTooltip(new Tooltip("The maximum velocity your robot can travel."))); + velocityUnits.textProperty() + .bind(lengthUnit.map(PathUnits.getInstance()::speedUnit).map(SimpleUnitFormat.getInstance()::format)); + accelerationControls + .forEach(control -> control.setTooltip(new Tooltip("The maximum capable acceleration of your robot."))); + accelerationUnits.textProperty().bind( + lengthUnit.map(PathUnits.getInstance()::accelerationUnit).map(SimpleUnitFormat.getInstance()::format)); + wheelBaseControls.forEach( + control -> control.setTooltip(new Tooltip("Distance between the left and right of the wheel base."))); + wheelBaseUnits.textProperty().bind(lengthUnit.map(SimpleUnitFormat.getInstance()::format)); + // Show longer text for an extended period of time + Stream.of(directoryControls, outputControls).flatMap(List::stream) + .forEach(control -> control.getTooltip().setShowDuration(Duration.seconds(10))); + Stream.of(directoryControls, outputControls, velocityControls, accelerationControls, + wheelBaseControls).flatMap(List::stream) + .forEach(control -> control.getTooltip().setShowDelay(Duration.millis(150))); + + // We are editing a project + if (ProjectPreferences.getInstance() != null) { + setupEditProject(); + } + } + + @FXML + private void createProject() { + String folderString = directory.getText().trim(); + // create a "PathWeaver" subdirectory if not editing an existing project + File directory = editing ? new File(folderString) : new File(folderString, "PathWeaver"); + directory.mkdir(); + String outputString = outputDirectory.getText(); + String outputPath = outputString; + boolean newOutput = !editing + || !Objects.equals(outputString, ProjectPreferences.getInstance().getValues().getOutputDir()); + if (outputString != null && !outputString.isEmpty() && newOutput) { + // Find the relative path for the output directory to the project directory, + // using / file separators + outputPath = directory.toPath().relativize(new File(outputString.trim()).toPath()).toString().replace("\\", + "/"); + } + ProgramPreferences.getInstance().addProject(directory.getAbsolutePath()); + String lengthUnit = length.getValue().getName(); + double velocityMax = Double.parseDouble(maxVelocity.getText()); + double accelerationMax = Double.parseDouble(maxAcceleration.getText()); + double wheelBaseDistance = Double.parseDouble(wheelBase.getText()); + ProjectPreferences.Values values = new ProjectPreferences.Values(lengthUnit, velocityMax, + accelerationMax, wheelBaseDistance, game.getValue().getName(), outputPath); + ProjectPreferences prefs = ProjectPreferences.getInstance(directory.getAbsolutePath()); + prefs.setValues(values); + editing = false; + FxUtils.loadMainScreen(vBox.getScene(), getClass()); + } + + @FXML + private void browseDirectory() { + browse(directory); + } + + @FXML + private void browseOutput() { + browse(outputDirectory); + } + + private void browse(TextField location) { + DirectoryChooser chooser = new DirectoryChooser(); + File selectedDirectory = chooser.showDialog(vBox.getScene().getWindow()); + if (selectedDirectory != null) { + location.setText(selectedDirectory.getPath()); + location.positionCaret(selectedDirectory.getPath().length()); + } + } + + @FXML + private void cancel() throws IOException { + Pane root = FXMLLoader.load(getClass().getResource("welcomeScreen.fxml")); + vBox.getScene().setRoot(root); + } + + private void setupEditProject() { + ProjectPreferences.Values values = ProjectPreferences.getInstance().getValues(); + directory.setText(ProjectPreferences.getInstance().getDirectory()); + outputDirectory.setText(ProjectPreferences.getInstance().getValues().getOutputDir()); + create.setText("Edit Project"); + title.setText("Edit Project"); + browse.setVisible(false); + cancel.setVisible(false); + game.setValue(Game.fromPrettyName(values.getGameName())); + length.setValue(values.getLengthUnit()); + maxVelocity.setText(String.valueOf(values.getMaxVelocity())); + maxAcceleration.setText(String.valueOf(values.getMaxAcceleration())); + wheelBase.setText(String.valueOf(values.getWheelBase())); + editing = true; + } } diff --git a/src/main/java/edu/wpi/first/pathweaver/Field.java b/src/main/java/edu/wpi/first/pathweaver/Field.java index 1e84f0a3..d469b995 100644 --- a/src/main/java/edu/wpi/first/pathweaver/Field.java +++ b/src/main/java/edu/wpi/first/pathweaver/Field.java @@ -1,145 +1,154 @@ package edu.wpi.first.pathweaver; -import tec.units.indriya.quantity.Quantities; - import java.util.function.Supplier; import javafx.geometry.Point2D; import javafx.scene.image.Image; +import tech.units.indriya.quantity.Quantities; import javax.measure.Quantity; import javax.measure.Unit; import javax.measure.quantity.Length; - public class Field { - private Supplier imageSupplier; - private Image image; - private Quantity rWidth; - private Quantity rLength; - private double xPixel; - private double yPixel; - private double pixelWidth; - private double pixelLength; - private double scale; - private Point2D coord; - public Unit unit; - - /** - * Creates a new Field Object. - * - * @param imageSupplier a supplier for image of the field - * @param unit unit which the field is measured - * @param realWidth width of field in real units - * @param realLength length of field in real units - * @param xPixel x pixel top left x pixels - * @param yPixel y pixel top left y pixels - * @param pixelWidth width of drawable area in pixels - * @param pixelLength length of drawable area in pixels - */ - public Field(Supplier imageSupplier, - Unit unit, - double realWidth, double realLength, - double xPixel, double yPixel, - double pixelWidth, double pixelLength) { - this.imageSupplier = imageSupplier; - this.xPixel = xPixel; - this.yPixel = yPixel; - this.pixelWidth = pixelWidth; - this.pixelLength = pixelLength; - setRealWidth(Quantities.getQuantity(realWidth, unit)); - setRealLength(Quantities.getQuantity(realLength, unit)); - updateCoord(); - updateScale(); - setUnit(unit); - } - - /** - * Creates a new Field Object. - * - * @param image image of the field - * @param unit unit which the field is measured - * @param realWidth width of field in real units - * @param realLength length of field in real units - * @param xPixel x pixel top left x pixels - * @param yPixel y pixel top left y pixels - * @param pixelWidth width of drawable area in pixels - * @param pixelLength length of drawable area in pixels - */ - public Field(Image image, - Unit unit, - double realWidth, double realLength, - double xPixel, double yPixel, - double pixelWidth, double pixelLength) { - this(() -> image, unit, realWidth, realLength, xPixel, yPixel, pixelWidth, pixelLength); - } - - public Image getImage() { - if (image == null) { - image = imageSupplier.get(); - } - return image; - } - - public Quantity getRealWidth() { - return rWidth; - } - - private void setRealWidth(Quantity width) { - this.rWidth = width; - } - - public Quantity getRealLength() { - return rLength; - } - - private void setRealLength(Quantity length) { - this.rLength = length; - } - - public double getScale() { - return scale; - } - - - public Point2D getCoord() { - return coord; - } - - - private double realWidth() { - return rWidth.getValue().doubleValue(); - } - - private double realLength() { - return rLength.getValue().doubleValue(); - } - - private void updateCoord() { - this.coord = new Point2D(xPixel + pixelWidth / 2 - realWidth() / 2, yPixel + pixelLength / 2 - realLength() / 2); - } - - private void updateScale() { - this.scale = ((pixelWidth / realWidth()) + (pixelLength / realLength())) / 2; - } - - public Unit getUnit() { - return unit; - } - - private void setUnit(Unit unit) { - this.unit = unit; - } - - /** - * Converts the Field from the current Unit systems to the supplied Unit system. - * @param unit The unit system to convert the Field to. - */ - public void convertUnit(Unit unit) { - setUnit(unit); - setRealWidth(rWidth.to(unit)); - setRealLength(rLength.to(unit)); - updateCoord(); - updateScale(); - } + private Supplier imageSupplier; + private Image image; + private Quantity rWidth; + private Quantity rLength; + private double xPixel; + private double yPixel; + private double pixelWidth; + private double pixelLength; + private double scale; + private Point2D coord; + public Unit unit; + + /** + * Creates a new Field Object. + * + * @param imageSupplier + * a supplier for image of the field + * @param unit + * unit which the field is measured + * @param realWidth + * width of field in real units + * @param realLength + * length of field in real units + * @param xPixel + * x pixel top left x pixels + * @param yPixel + * y pixel top left y pixels + * @param pixelWidth + * width of drawable area in pixels + * @param pixelLength + * length of drawable area in pixels + */ + public Field(Supplier imageSupplier, Unit unit, double realWidth, double realLength, double xPixel, + double yPixel, double pixelWidth, double pixelLength) { + this.imageSupplier = imageSupplier; + this.xPixel = xPixel; + this.yPixel = yPixel; + this.pixelWidth = pixelWidth; + this.pixelLength = pixelLength; + setRealWidth(Quantities.getQuantity(realWidth, unit)); + setRealLength(Quantities.getQuantity(realLength, unit)); + updateCoord(); + updateScale(); + setUnit(unit); + } + + /** + * Creates a new Field Object. + * + * @param image + * image of the field + * @param unit + * unit which the field is measured + * @param realWidth + * width of field in real units + * @param realLength + * length of field in real units + * @param xPixel + * x pixel top left x pixels + * @param yPixel + * y pixel top left y pixels + * @param pixelWidth + * width of drawable area in pixels + * @param pixelLength + * length of drawable area in pixels + */ + public Field(Image image, Unit unit, double realWidth, double realLength, double xPixel, double yPixel, + double pixelWidth, double pixelLength) { + this(() -> image, unit, realWidth, realLength, xPixel, yPixel, pixelWidth, pixelLength); + } + + public Image getImage() { + if (image == null) { + image = imageSupplier.get(); + } + return image; + } + + public Quantity getRealWidth() { + return rWidth; + } + + private void setRealWidth(Quantity width) { + this.rWidth = width; + } + + public Quantity getRealLength() { + return rLength; + } + + private void setRealLength(Quantity length) { + this.rLength = length; + } + + public double getScale() { + return scale; + } + + public Point2D getCoord() { + return coord; + } + + private double realWidth() { + return rWidth.getValue().doubleValue(); + } + + private double realLength() { + return rLength.getValue().doubleValue(); + } + + private void updateCoord() { + this.coord = new Point2D(xPixel + pixelWidth / 2 - realWidth() / 2, + yPixel + pixelLength / 2 - realLength() / 2); + } + + private void updateScale() { + this.scale = ((pixelWidth / realWidth()) + (pixelLength / realLength())) / 2; + } + + public Unit getUnit() { + return unit; + } + + private void setUnit(Unit unit) { + this.unit = unit; + } + + /** + * Converts the Field from the current Unit systems to the supplied Unit system. + * + * @param unit + * The unit system to convert the Field to. + */ + public void convertUnit(Unit unit) { + setUnit(unit); + setRealWidth(rWidth.to(unit)); + setRealLength(rLength.to(unit)); + updateCoord(); + updateScale(); + } } diff --git a/src/main/java/edu/wpi/first/pathweaver/FieldDisplayController.java b/src/main/java/edu/wpi/first/pathweaver/FieldDisplayController.java index da822e17..92bcae0c 100644 --- a/src/main/java/edu/wpi/first/pathweaver/FieldDisplayController.java +++ b/src/main/java/edu/wpi/first/pathweaver/FieldDisplayController.java @@ -164,11 +164,12 @@ public Path duplicate(String pathDirectory) { /** * Checks if the given x, y coordinates are within the valid area of the drawpane. * - * @param x X coordinate - * @param y Y coordinate + * @param x X coordinate, following {@link Waypoint} convention + * @param y Y coordinate following {@link Waypoint} convention * @return True if X, Y is within the bounds of the drawpane. */ public boolean checkBounds(double x, double y) { - return drawPane.getLayoutBounds().contains(x, y); + //Convert waypoint convention to JavaFX + return drawPane.getLayoutBounds().contains(x, -y); } } diff --git a/src/main/java/edu/wpi/first/pathweaver/MainController.java b/src/main/java/edu/wpi/first/pathweaver/MainController.java index 87384ff9..6f1d1853 100644 --- a/src/main/java/edu/wpi/first/pathweaver/MainController.java +++ b/src/main/java/edu/wpi/first/pathweaver/MainController.java @@ -1,7 +1,7 @@ package edu.wpi.first.pathweaver; -import java.io.File; import java.io.IOException; +import java.nio.file.Files; import java.util.ArrayList; import java.util.List; import java.util.logging.Level; @@ -330,26 +330,27 @@ private void buildPaths() { if (!SaveManager.getInstance().promptSaveAll()) { return; } - File output = ProjectPreferences.getInstance().getOutputDir(); - output.mkdirs(); + + java.nio.file.Path output = ProjectPreferences.getInstance().getOutputDir().toPath(); + try { + Files.createDirectories(output); + } catch (IOException e) { + LOGGER.log(Level.WARNING, "Could not export to " + output, e); + } for (TreeItem pathName : pathRoot.getChildren()) { Path path = PathIOUtil.importPath(pathDirectory, pathName.getValue()); - File f = new File(output, path.getPathNameNoExtension() + ".wpilib.csv"); + java.nio.file.Path pathNameFile = output.resolve(path.getPathNameNoExtension()); - if(!path.getSpline().writeToFile(f.toPath())) { + if(!path.getSpline().writeToFile(pathNameFile)) { Alert alert = new Alert(Alert.AlertType.WARNING); alert.setTitle("Path export failure!"); - alert.setContentText("Could not export to: " + f.getAbsolutePath()); + alert.setContentText("Could not export to: " + output.toAbsolutePath()); } } Alert alert = new Alert(Alert.AlertType.INFORMATION); alert.setTitle("Paths exported!"); - try { - alert.setContentText("Paths exported to: " + output.getCanonicalPath()); - } catch (IOException e) { - LOGGER.log(Level.WARNING, "Could not export to " + output.getPath(), e); - } + alert.show(); } diff --git a/src/main/java/edu/wpi/first/pathweaver/PathUnits.java b/src/main/java/edu/wpi/first/pathweaver/PathUnits.java index 86a47453..0ff995dc 100644 --- a/src/main/java/edu/wpi/first/pathweaver/PathUnits.java +++ b/src/main/java/edu/wpi/first/pathweaver/PathUnits.java @@ -1,133 +1,194 @@ package edu.wpi.first.pathweaver; +import si.uom.SI; import systems.uom.common.USCustomary; -import tec.units.ri.AbstractSystemOfUnits; -import tec.units.ri.AbstractUnit; -import tec.units.ri.format.SimpleUnitFormat; +import tech.units.indriya.AbstractSystemOfUnits; +import tech.units.indriya.AbstractUnit; +import tech.units.indriya.format.SimpleUnitFormat; +import java.util.Collections; import java.util.List; import java.util.Locale; +import javax.measure.MetricPrefix; import javax.measure.Unit; -import javax.measure.quantity.Length; -import javax.measure.quantity.Time; +import javax.measure.quantity.*; public final class PathUnits extends AbstractSystemOfUnits { - private static final String SYSTEM_NAME = "PathWeaver Units"; - private static final PathUnits INSTANCE = new PathUnits(); - - public static final Unit METER = addUnit(USCustomary.METER, "Meter", "m"); - public static final Unit CENTIMETER = addUnit(METER.divide(100), "Centimeter", "cm"); - public static final Unit MILLIMETER = addUnit(METER.divide(1000), "Millimeter", "mm"); - public static final Unit INCH = addUnit(USCustomary.INCH, "Inch", "in"); - public static final Unit FOOT = addUnit(USCustomary.FOOT, "Foot", "ft"); - public static final Unit YARD = addUnit(USCustomary.YARD, "Yard", "yd"); - public static final Unit MILE = addUnit(USCustomary.MILE, "Mile", "mi"); - - public static final List> LENGTHS = List.of(METER, CENTIMETER, MILLIMETER, INCH, FOOT, YARD, MILE); - - public static final Unit