Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

Fix paths with multiple consecutive removable points

Browse Source
This commit is contained in:
nathanrsxtn
2022-02-28 18:57:53 -07:00
parent 40a4d6d369
commit bd6025bf52
2 changed files with 5 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+4 -6
View File
@@ -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