From f0778e7592041ffef6bdd5b8c790810d49951dc3 Mon Sep 17 00:00:00 2001 From: Andrew Dassonville Date: Mon, 8 Oct 2018 18:44:47 -0700 Subject: [PATCH 1/8] Show robot --- .../pathweaver/CreateProjectController.java | 8 +++++- .../pathweaver/PathDisplayController.java | 15 ++++++++--- .../first/pathweaver/ProjectPreferences.java | 26 +++++++++++++++++-- .../edu/wpi/first/pathweaver/Waypoint.java | 22 +++++++++++++++- .../wpi/first/pathweaver/createProject.fxml | 10 +++++-- .../edu/wpi/first/pathweaver/style.css | 7 ++++- .../wpi/first/pathweaver/welcomeScreen.fxml | 2 +- 7 files changed, 79 insertions(+), 11 deletions(-) diff --git a/src/main/java/edu/wpi/first/pathweaver/CreateProjectController.java b/src/main/java/edu/wpi/first/pathweaver/CreateProjectController.java index c178b780..41bda314 100644 --- a/src/main/java/edu/wpi/first/pathweaver/CreateProjectController.java +++ b/src/main/java/edu/wpi/first/pathweaver/CreateProjectController.java @@ -32,6 +32,10 @@ public class CreateProjectController { private TextField maxJerk; @FXML private TextField wheelBase; + @FXML + private TextField robotWidth; + @FXML + private TextField robotLength; @FXML private void initialize() { @@ -64,8 +68,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); + jerkMax, wheelBaseDistance, robotWidthValue, robotLengthValue); ProjectPreferences prefs = ProjectPreferences.getInstance(folder); prefs.setValues(values); FxUtils.loadMainScreen(vBox.getScene(), getClass()); diff --git a/src/main/java/edu/wpi/first/pathweaver/PathDisplayController.java b/src/main/java/edu/wpi/first/pathweaver/PathDisplayController.java index d7d1bf60..b08ff74a 100644 --- a/src/main/java/edu/wpi/first/pathweaver/PathDisplayController.java +++ b/src/main/java/edu/wpi/first/pathweaver/PathDisplayController.java @@ -17,6 +17,7 @@ import javafx.scene.input.KeyCode; import javafx.scene.input.KeyEvent; import javafx.scene.layout.Pane; +import javafx.scene.paint.Paint; import javafx.scene.transform.Scale; @SuppressWarnings("PMD.TooManyMethods") @@ -40,7 +41,7 @@ public class PathDisplayController { private String pathDirectory; 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 splineScale = 8; //NOPMD should be static, will be modified later private final double lineScale = 2; //NOPMD should be static, will be modified later @FXML @@ -140,12 +141,18 @@ 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.getRobotOutline().getStyleClass().add("robotOutline"); + current.getRobotOutline().setStroke(Paint.valueOf("lightgray")); + current.getRobotOutline().setStrokeWidth(lineScale / field.getScale()); + current.getRobotOutline().toBack(); current.getTangentLine().setStrokeWidth(lineScale / field.getScale()); - current.getTangentLine().toBack(); - current.getSpline().addToGroup(splineGroup, splineScale / field.getScale()); + current.getTangentLine().toFront(); + current.getSpline().addToGroup(splineGroup, current.getRobotOutline().getHeight()); + current.getSpline().addToGroup(splineGroup, 40); } @@ -155,6 +162,7 @@ private void removePathFromPane(Path newPath) { } for (Waypoint wp : newPath.getWaypoints()) { waypointGroup.getChildren().remove(wp.getIcon()); + waypointGroup.getChildren().remove(wp.getRobotOutline()); vectorGroup.getChildren().remove(wp.getTangentLine()); } } @@ -218,6 +226,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 a240b4d3..5642de52 100644 --- a/src/main/java/edu/wpi/first/pathweaver/ProjectPreferences.java +++ b/src/main/java/edu/wpi/first/pathweaver/ProjectPreferences.java @@ -34,7 +34,7 @@ private ProjectPreferences(String directory) { } private void setDefaults() { - values = new Values(0.2, 10.0, 60.0, 60.0, 2.0); + values = new Values(0.2, 10.0, 60.0, 60.0, 2.0, 28.0, 38.0); updateValues(); } @@ -59,6 +59,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; } @@ -95,6 +103,8 @@ public static class Values { private final double maxAcceleration; private final double maxJerk; private final double wheelBase; + private final double robotWidth; + private final double robotLength; /** * Constructor for Values of ProjectPreferences. @@ -103,14 +113,18 @@ 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) */ public Values(double timeStep, double maxVelocity, double maxAcceleration, double maxJerk, - double wheelBase) { + double wheelBase, double robotWidth, double robotLength) { this.timeStep = timeStep; this.maxVelocity = maxVelocity; this.maxAcceleration = maxAcceleration; this.maxJerk = maxJerk; this.wheelBase = wheelBase; + this.robotWidth = robotWidth; + this.robotLength = robotLength; } public double getTimeStep() { @@ -132,5 +146,13 @@ public double getMaxJerk() { public double getWheelBase() { return wheelBase; } + + public double getRobotWidth() { + return robotWidth; + } + + public double getRobotLength() { + return robotLength; + } } } diff --git a/src/main/java/edu/wpi/first/pathweaver/Waypoint.java b/src/main/java/edu/wpi/first/pathweaver/Waypoint.java index 41562ba3..3535a9ed 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; import javafx.util.Duration; 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; @@ -64,6 +65,8 @@ public Waypoint(Point2D position, Point2D tangentVector, boolean fixedAngle, Pat x.addListener(__ -> update()); y.addListener(__ -> update()); + ProjectPreferences.Values values = ProjectPreferences.getInstance().getValues(); + tangentLine = new Line(); tangentLine.getStyleClass().add("tangent"); tangentLine.startXProperty().bind(x); @@ -72,6 +75,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() / 12; + double robotLength = values.getRobotLength() / 12; + + 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(); @@ -184,6 +200,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 6d787b74..9f8c8326 100644 --- a/src/main/resources/edu/wpi/first/pathweaver/createProject.fxml +++ b/src/main/resources/edu/wpi/first/pathweaver/createProject.fxml @@ -11,7 +11,7 @@ - +