diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java index a1d6237d6d..7f35daeb65 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java @@ -120,8 +120,11 @@ public class RamseteController { final double vRef = linearVelocityRefMeters; final double omegaRef = angularVelocityRefRadiansPerSecond; + // k = 2ζ√(ω_ref² + b v_ref²) double k = 2.0 * m_zeta * Math.sqrt(Math.pow(omegaRef, 2) + m_b * Math.pow(vRef, 2)); + // v_cmd = v_ref cos(e_θ) + k e_x + // ω_cmd = ω_ref + k e_θ + b v_ref sinc(e_θ) e_y return new ChassisSpeeds( vRef * m_poseError.getRotation().getCos() + k * eX, 0.0, diff --git a/wpimath/src/main/native/cpp/controller/RamseteController.cpp b/wpimath/src/main/native/cpp/controller/RamseteController.cpp index 337739d538..fdd7d63bf8 100644 --- a/wpimath/src/main/native/cpp/controller/RamseteController.cpp +++ b/wpimath/src/main/native/cpp/controller/RamseteController.cpp @@ -63,10 +63,13 @@ ChassisSpeeds RamseteController::Calculate( const auto& vRef = linearVelocityRef; const auto& omegaRef = angularVelocityRef; + // k = 2ζ√(ω_ref² + b v_ref²) auto k = 2.0 * m_zeta * units::math::sqrt(units::math::pow<2>(omegaRef) + m_b * units::math::pow<2>(vRef)); + // v_cmd = v_ref cos(e_θ) + k e_x + // ω_cmd = ω_ref + k e_θ + b v_ref sinc(e_θ) e_y return ChassisSpeeds{vRef * m_poseError.Rotation().Cos() + k * eX, 0_mps, omegaRef + k * eTheta + m_b * vRef * Sinc(eTheta) * eY}; }