Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add comments with Ramsete equations (#4348)

Browse Source
This commit is contained in:
Tyler Veness
2022-08-11 07:09:16 -07:00
committed by GitHub
parent 0485f05da9
commit 3fdb2f767d
2 changed files with 6 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
wpimath/src/main/native/cpp/controller/RamseteController.cpp
View File

@@ -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};
}