From 3fdb2f767d466e00d19e487fdb64d33c22ccc7d5 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Thu, 11 Aug 2022 07:09:16 -0700 Subject: [PATCH] [wpimath] Add comments with Ramsete equations (#4348) --- .../java/edu/wpi/first/math/controller/RamseteController.java | 3 +++ wpimath/src/main/native/cpp/controller/RamseteController.cpp | 3 +++ 2 files changed, 6 insertions(+) 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}; }