Increase acceptable pathfinding heading error

mjansen4857 · Dec 25, 2023 · b2e2081 · b2e2081
1 parent 5d65d73
commit b2e2081
Show file tree

Hide file tree

Showing 3 changed files with 3 additions and 3 deletions.
diff --git a/pathplannerlib-python/pathplannerlib/commands.py b/pathplannerlib-python/pathplannerlib/commands.py
@@ -417,7 +417,7 @@ def execute(self):
                 fieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(currentSpeeds, currentPose.rotation())
                 currentHeading = Rotation2d(fieldRelativeSpeeds.vx, fieldRelativeSpeeds.vy)
                 headingError = currentHeading - closestState1.heading
-                onHeading = math.hypot(currentSpeeds.vx, currentSpeeds.vy) < 1.0 or abs(headingError.degrees()) < 30
+                onHeading = math.hypot(currentSpeeds.vx, currentSpeeds.vy) < 1.0 or abs(headingError.degrees()) < 45
 
                 # Replan the path if our heading is off
                 if onHeading or not self._replanningConfig.enableInitialReplanning:

diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java
@@ -227,7 +227,7 @@ public void execute() {
         Rotation2d headingError = currentHeading.minus(closestState1.heading);
         boolean onHeading =
             Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond) < 1.0
-                || Math.abs(headingError.getDegrees()) < 30;
+                || Math.abs(headingError.getDegrees()) < 45;
 
         // Replan the path if our heading is off
         if (onHeading || !replanningConfig.enableInitialReplanning) {

diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp
@@ -146,7 +146,7 @@ void PathfindingCommand::Execute() {
 					- m_currentPath->getStartingDifferentialPose().Rotation();
 			bool onHeading = units::math::hypot(currentSpeeds.vx,
 					currentSpeeds.vy) < 1.0_mps
-					|| units::math::abs(headingError.Degrees()) < 30_deg;
+					|| units::math::abs(headingError.Degrees()) < 45_deg;
 
 			// Replan the path if our heading is off
 			if (onHeading || !m_replanningConfig.enableInitialReplanning) {