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/java/edu/wpi/first/math/controller/RamseteController.java
View File

@@ -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,