fix pathfinding waypoint generation

mjansen4857 · Oct 5, 2024 · 66441af · 66441af
1 parent add8edd
commit 66441af
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 6 deletions.
diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/LocalADStar.java b/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/LocalADStar.java
@@ -427,7 +427,7 @@ private List<Waypoint> createWaypoints(
       Translation2d anchor1 = current.minus(last).times(SMOOTHING_ANCHOR_PCT).plus(last);
       Rotation2d heading1 = current.minus(last).getAngle();
       Translation2d anchor2 = current.minus(next).times(SMOOTHING_ANCHOR_PCT).plus(next);
-      Rotation2d heading2 = anchor2.minus(next).getAngle();
+      Rotation2d heading2 = next.minus(anchor2).getAngle();
 
       pathPoses.add(new Pose2d(anchor1, heading1));
       pathPoses.add(new Pose2d(anchor2, heading2));
@@ -436,8 +436,8 @@ private List<Waypoint> createWaypoints(
         new Pose2d(
             fieldPosPath.get(fieldPosPath.size() - 1),
             fieldPosPath
-                .get(fieldPosPath.size() - 2)
-                .minus(fieldPosPath.get(fieldPosPath.size() - 1))
+                .get(fieldPosPath.size() - 1)
+                .minus(fieldPosPath.get(fieldPosPath.size() - 2))
                 .getAngle()));
 
     return PathPlannerPath.waypointsFromPoses(pathPoses);

diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/LocalADStar.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/LocalADStar.cpp
@@ -389,14 +389,14 @@ std::vector<Waypoint> LocalADStar::createWaypoints(
 		frc::Rotation2d heading1 = (current - last).Angle();
 		frc::Translation2d anchor2 = ((current - next) * SMOOTHING_ANCHOR_PCT)
 				+ next;
-		frc::Rotation2d heading2 = (anchor2 - next).Angle();
+		frc::Rotation2d heading2 = (next - anchor2).Angle();
 
 		pathPoses.emplace_back(anchor1, heading1);
 		pathPoses.emplace_back(anchor2, heading2);
 	}
 	pathPoses.emplace_back(fieldPosPath[fieldPosPath.size() - 1],
-			(fieldPosPath[fieldPosPath.size() - 2]
-					- fieldPosPath[fieldPosPath.size() - 1]).Angle());
+			(fieldPosPath[fieldPosPath.size() - 1]
+					- fieldPosPath[fieldPosPath.size() - 2]).Angle());
 
 	return PathPlannerPath::waypointsFromPoses(pathPoses);
 }