diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java index b77c259e92..5b3bd91d9a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.util.Units; /** * This holonomic drive controller can be used to follow trajectories using a holonomic drivetrain @@ -44,6 +45,7 @@ public class HolonomicDriveController { m_xController = xController; m_yController = yController; m_thetaController = thetaController; + m_thetaController.enableContinuousInput(0, Units.degreesToRadians(360.0)); } /** diff --git a/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp b/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp index 6a09c57fa5..de0414778e 100644 --- a/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp +++ b/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp @@ -15,7 +15,9 @@ HolonomicDriveController::HolonomicDriveController( ProfiledPIDController thetaController) : m_xController(std::move(xController)), m_yController(std::move(yController)), - m_thetaController(std::move(thetaController)) {} + m_thetaController(std::move(thetaController)) { + m_thetaController.EnableContinuousInput(0_deg, 360.0_deg); +} bool HolonomicDriveController::AtReference() const { const auto& eTranslate = m_poseError.Translation();