From acb64dff97ba6d0091c9ce87c126c96a6c6de796 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Mon, 6 Dec 2021 12:57:42 -0800 Subject: [PATCH] [wpimath] Make RamseteController::Calculate() more concise (#3763) --- .../src/main/native/cpp/controller/RamseteController.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/wpimath/src/main/native/cpp/controller/RamseteController.cpp b/wpimath/src/main/native/cpp/controller/RamseteController.cpp index 55c4815fe1..0fea864a71 100644 --- a/wpimath/src/main/native/cpp/controller/RamseteController.cpp +++ b/wpimath/src/main/native/cpp/controller/RamseteController.cpp @@ -71,10 +71,8 @@ ChassisSpeeds RamseteController::Calculate( units::math::sqrt(units::math::pow<2>(omegaRef) + m_b * units::math::pow<2>(vRef)); - units::meters_per_second_t v{vRef * m_poseError.Rotation().Cos() + k * eX}; - units::radians_per_second_t omega{omegaRef + k * eTheta + - m_b * vRef * Sinc(eTheta) * eY}; - return ChassisSpeeds{v, 0_mps, omega}; + return ChassisSpeeds{vRef * m_poseError.Rotation().Cos() + k * eX, 0_mps, + omegaRef + k * eTheta + m_b * vRef * Sinc(eTheta) * eY}; } ChassisSpeeds RamseteController::Calculate(