From bd6025bf5290c6a48c458d800e642dce645b3835 Mon Sep 17 00:00:00 2001 From: nathanrsxtn Date: Mon, 28 Feb 2022 18:57:53 -0700 Subject: [PATCH] Fix paths with multiple consecutive removable points --- .gitignore | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 10 ++++------ 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/.gitignore b/.gitignore index c36585d..5e48910 100644 --- a/.gitignore +++ b/.gitignore @@ -159,6 +159,6 @@ gradle-app.setting bin/ # Simulation GUI and other tools window save file -simgui-*.json +simgui*.json src/main/deploy/config.json \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 86231d4..15c9738 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -275,8 +275,7 @@ public class RobotContainer { public PathPlannerUtil.Path createPath(Double maxVelocity, Double maxAcceleration, Boolean isReversed) { // pathPoints = Arrays.stream(PathPlannerUtil.Path.read(autoChooser.getSelected()).waypoints.get()).collect(Collectors.toList()); - // Remove points whose angles to neighboring points are less than 10 degrees - // apart. + // Remove points whose angles to neighboring points are less than 10 degrees apart. int j = 0; for (int i = 1; i < pathPoints.size() - 1; i++) { var prev = pathPoints.get(j).anchorPoint.orElseThrow(); @@ -286,11 +285,10 @@ public class RobotContainer { var toNext = next.minus(current); var angleFromPrevious = new Rotation2d(fromPrevious.getX(), fromPrevious.getY()); var angleToNext = new Rotation2d(toNext.getX(), toNext.getY()); - j = i; - if (Math.abs(angleFromPrevious.minus(angleToNext).getDegrees()) < 20) { + if (Math.abs(angleFromPrevious.minus(angleToNext).getDegrees()) < 20) pathPoints.set(i, null); - j--; - } + else + j = i; } pathPoints.removeIf(Objects::isNull); // Make control points