diff --git a/pathplannerlib-python/pathplannerlib/commands.py b/pathplannerlib-python/pathplannerlib/commands.py index 30824712..d0811e9a 100644 --- 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 index 37472f40..ff05e01c 100644 --- 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 index 5d8963d0..a4e6ae09 100644 --- 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) {