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 e43c6ff3d4..87148a45db 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 @@ -139,4 +139,31 @@ public class HolonomicDriveController { public void setEnabled(boolean enabled) { m_enabled = enabled; } + + /** + * Returns the heading controller. + * + * @return heading ProfiledPIDController + */ + public ProfiledPIDController getThetaController() { + return m_thetaController; + } + + /** + * Returns the x controller. + * + * @return X PIDController + */ + public PIDController getXController() { + return m_xController; + } + + /** + * Returns the y controller. + * + * @return Y PIDController + */ + public PIDController getYController() { + return m_yController; + } } diff --git a/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp b/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp index def723470f..7a4c27bc77 100644 --- a/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp +++ b/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp @@ -79,3 +79,16 @@ ChassisSpeeds HolonomicDriveController::Calculate( void HolonomicDriveController::SetEnabled(bool enabled) { m_enabled = enabled; } + +ProfiledPIDController& +HolonomicDriveController::getThetaController() { + return m_thetaController; +} + +PIDController& HolonomicDriveController::getXController() { + return m_xController; +} + +PIDController& HolonomicDriveController::getYController() { + return m_yController; +} diff --git a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h index 9a749c6f0d..4831e9ca69 100644 --- a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h +++ b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h @@ -103,6 +103,21 @@ class WPILIB_DLLEXPORT HolonomicDriveController { */ void SetEnabled(bool enabled); + /** + * Returns the rotation ProfiledPIDController + */ + ProfiledPIDController& getThetaController(); + + /** + * Returns the X PIDController + */ + PIDController& getXController(); + + /** + * Returns the Y PIDController + */ + PIDController& getYController(); + private: Pose2d m_poseError; Rotation2d m_rotationError;