diff --git a/src/main/java/edu/wpi/first/pathweaver/CreateProjectController.java b/src/main/java/edu/wpi/first/pathweaver/CreateProjectController.java index c0a26202..54dae0aa 100644 --- a/src/main/java/edu/wpi/first/pathweaver/CreateProjectController.java +++ b/src/main/java/edu/wpi/first/pathweaver/CreateProjectController.java @@ -40,6 +40,10 @@ public class CreateProjectController { @FXML private TextField wheelBase; @FXML + private TextField robotWidth; + @FXML + private TextField robotLength; + @FXML private ChoiceBox game; @FXML @@ -90,8 +94,10 @@ private void createProject() { double accelerationMax = Double.parseDouble(maxAcceleration.getText()); double jerkMax = Double.parseDouble(maxJerk.getText()); double wheelBaseDistance = Double.parseDouble(wheelBase.getText()); + double robotWidthValue = Double.parseDouble(robotWidth.getText()); + double robotLengthValue = Double.parseDouble(robotLength.getText()); ProjectPreferences.Values values = new ProjectPreferences.Values(timeDelta, velocityMax, accelerationMax, - jerkMax, wheelBaseDistance, game.getValue()); + jerkMax, wheelBaseDistance, robotWidthValue, robotLengthValue, game.getValue()); ProjectPreferences prefs = ProjectPreferences.getInstance(directory.getAbsolutePath()); prefs.setValues(values); FxUtils.loadMainScreen(vBox.getScene(), getClass()); @@ -124,5 +130,7 @@ private void setupEditProject() { maxAcceleration.setText(String.valueOf(values.getMaxAcceleration())); maxJerk.setText(String.valueOf(values.getMaxJerk())); wheelBase.setText(String.valueOf(values.getWheelBase())); + robotWidth.setText(String.valueOf(values.getRobotWidth())); + robotLength.setText(String.valueOf(values.getRobotLength())); } } diff --git a/src/main/java/edu/wpi/first/pathweaver/PathDisplayController.java b/src/main/java/edu/wpi/first/pathweaver/PathDisplayController.java index cc5e9556..4884735e 100644 --- a/src/main/java/edu/wpi/first/pathweaver/PathDisplayController.java +++ b/src/main/java/edu/wpi/first/pathweaver/PathDisplayController.java @@ -43,7 +43,7 @@ public class PathDisplayController { private final double circleScale = .75; //NOPMD should be static, will be modified later private final double splineScale = 6; //NOPMD should be static, will be modified later - private final double lineScale = 4; //NOPMD should be static, will be modified later + private final double lineScale = 2; //NOPMD should be static, will be modified later @FXML private Group splineGroup; @@ -142,21 +142,25 @@ private void addPathToPane(Path newPath) { */ public void addWaypointToPane(Waypoint current) { waypointGroup.getChildren().add(current.getIcon()); + waypointGroup.getChildren().add(current.getRobotOutline()); vectorGroup.getChildren().add(current.getTangentLine()); current.getIcon().setScaleX(circleScale / field.getScale()); current.getIcon().setScaleY(circleScale / field.getScale()); + current.getIcon().toFront(); + current.getRobotOutline().getStyleClass().add("robotOutline"); + current.getRobotOutline().setStrokeWidth(lineScale / field.getScale()); current.getTangentLine().setStrokeWidth(lineScale / field.getScale()); - current.getTangentLine().toBack(); + current.getTangentLine().toFront(); current.getSpline().addToGroup(splineGroup, splineScale / field.getScale()); } - private void removePathFromPane(Path newPath) { for (Waypoint wp : newPath.getWaypoints()) { wp.getSpline().removeFromGroup(splineGroup); } for (Waypoint wp : newPath.getWaypoints()) { waypointGroup.getChildren().remove(wp.getIcon()); + waypointGroup.getChildren().remove(wp.getRobotOutline()); vectorGroup.getChildren().remove(wp.getTangentLine()); } } @@ -225,6 +229,7 @@ private void delete(Waypoint waypoint) { Path path = waypoint.getPath(); Waypoint previous = path.getWaypoints().get(path.getWaypoints().indexOf(waypoint) - 1); waypointGroup.getChildren().remove(waypoint.getIcon()); + waypointGroup.getChildren().remove(waypoint.getRobotOutline()); vectorGroup.getChildren().remove(waypoint.getTangentLine()); waypoint.getSpline().removeFromGroup(splineGroup); path.remove(waypoint); diff --git a/src/main/java/edu/wpi/first/pathweaver/ProjectPreferences.java b/src/main/java/edu/wpi/first/pathweaver/ProjectPreferences.java index bb133e3a..74a79037 100644 --- a/src/main/java/edu/wpi/first/pathweaver/ProjectPreferences.java +++ b/src/main/java/edu/wpi/first/pathweaver/ProjectPreferences.java @@ -37,7 +37,7 @@ private ProjectPreferences(String directory) { } private void setDefaults() { - values = new Values(0.2, 10.0, 60.0, 60.0, 2.0, Game.POWER_UP_2018); + values = new Values(0.2, 10.0, 60.0, 60.0, 2.0, 28.0, 38.0, Game.POWER_UP_2018); updateValues(); } @@ -62,6 +62,14 @@ public void setValues(Values values) { updateValues(); } + /** + * Gets the preferences for the current project. + * @return the preference values for this project. + */ + public Values getValues() { + return values; + } + public String getDirectory() { return directory; } @@ -115,16 +123,14 @@ public Field getField() { } } - public Values getValues() { - return values; - } - public static class Values { private final double timeStep; private final double maxVelocity; private final double maxAcceleration; private final double maxJerk; private final double wheelBase; + private final double robotWidth; + private final double robotLength; private Game game; /** @@ -134,15 +140,19 @@ public static class Values { * @param maxAcceleration The maximum acceleration to use * @param maxJerk The maximum jerk (acceleration per second) to use * @param wheelBase The width between the individual sides of the drivebase + * @param robotWidth The width of the robot (in inches) + * @param robotLength The length of the robot (in inches) * @param game The year/FRC game */ public Values(double timeStep, double maxVelocity, double maxAcceleration, double maxJerk, - double wheelBase, Game game) { + double wheelBase, double robotWidth, double robotLength, Game game) { this.timeStep = timeStep; this.maxVelocity = maxVelocity; this.maxAcceleration = maxAcceleration; this.maxJerk = maxJerk; this.wheelBase = wheelBase; + this.robotWidth = robotWidth; + this.robotLength = robotLength; this.game = game; } @@ -166,6 +176,14 @@ public double getWheelBase() { return wheelBase; } + public double getRobotWidth() { + return robotWidth; + } + + public double getRobotLength() { + return robotLength; + } + public Game getGame() { return game; } diff --git a/src/main/java/edu/wpi/first/pathweaver/Waypoint.java b/src/main/java/edu/wpi/first/pathweaver/Waypoint.java index 49a4bc42..4b6a8835 100644 --- a/src/main/java/edu/wpi/first/pathweaver/Waypoint.java +++ b/src/main/java/edu/wpi/first/pathweaver/Waypoint.java @@ -15,6 +15,7 @@ import javafx.scene.input.TransferMode; import javafx.scene.shape.Line; import javafx.scene.shape.Polygon; +import javafx.scene.shape.Rectangle; @SuppressWarnings("PMD.TooManyMethods") public class Waypoint { @@ -30,7 +31,7 @@ public class Waypoint { private Path path; public static Waypoint currentWaypoint = null; - + private final Rectangle robotOutline; private final Line tangentLine; private Polygon icon; @@ -52,6 +53,7 @@ public void setPath(Path path) { * @param fixedAngle If the angle the of the waypoint should be fixed. Used for first and last waypoint * @param myPath the path this waypoint belongs to */ + @SuppressWarnings("PMD.NcssCount") public Waypoint(Point2D position, Point2D tangentVector, boolean fixedAngle, Path myPath) { path = myPath; lockTangent.set(fixedAngle); @@ -62,6 +64,8 @@ public Waypoint(Point2D position, Point2D tangentVector, boolean fixedAngle, Pat x.addListener(listener -> update()); y.addListener(listener -> update()); + ProjectPreferences.Values values = ProjectPreferences.getInstance().getValues(); + tangentLine = new Line(); tangentLine.getStyleClass().add("tangent"); tangentLine.startXProperty().bind(x); @@ -70,6 +74,19 @@ public Waypoint(Point2D position, Point2D tangentVector, boolean fixedAngle, Pat tangentLine.endXProperty().bind(Bindings.createObjectBinding(() -> getTangent().getX() + getX(), tangentX, x)); tangentLine.endYProperty().bind(Bindings.createObjectBinding(() -> getTangent().getY() + getY(), tangentY, y)); + double robotWidth = values.getRobotWidth(); + double robotLength = values.getRobotLength(); + + robotOutline = new Rectangle(); + robotOutline.setHeight(robotWidth); + robotOutline.setWidth(robotLength); + robotOutline.xProperty().bind(x.subtract(robotLength / 2)); + robotOutline.yProperty().bind(y.subtract(robotWidth / 2)); + robotOutline.rotateProperty().bind( + Bindings.createObjectBinding(() -> + getTangent() == null ? 0.0 : Math.toDegrees(Math.atan2(getTangent().getY(), getTangent().getX())), + tangentX, tangentY)); + this.spline = new NullSpline(); setupDnd(); @@ -182,6 +199,10 @@ public void setTangent(Point2D tangent) { this.tangentY.set(tangent.getY()); } + public Rectangle getRobotOutline() { + return robotOutline; + } + public Polygon getIcon() { return icon; } diff --git a/src/main/resources/edu/wpi/first/pathweaver/createProject.fxml b/src/main/resources/edu/wpi/first/pathweaver/createProject.fxml index cb6a5103..d282f500 100644 --- a/src/main/resources/edu/wpi/first/pathweaver/createProject.fxml +++ b/src/main/resources/edu/wpi/first/pathweaver/createProject.fxml @@ -12,7 +12,7 @@ - +