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(