[wpimath] Document ChassisSpeeds::Discretize() math (NFC) (#6509)

This commit is contained in:
Tyler Veness
2024-04-10 22:03:44 -07:00
committed by GitHub
parent 3a5d24ab1d
commit 2def62a1ef
2 changed files with 14 additions and 0 deletions

View File

@@ -61,8 +61,15 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
units::meters_per_second_t vy,
units::radians_per_second_t omega,
units::second_t dt) {
// Construct the desired pose after a timestep, relative to the current
// pose. The desired pose has decoupled translation and rotation.
Pose2d desiredDeltaPose{vx * dt, vy * dt, omega * dt};
// Find the chassis translation/rotation deltas in the robot frame that move
// the robot from its current pose to the desired pose
auto twist = Pose2d{}.Log(desiredDeltaPose);
// Turn the chassis translation/rotation deltas into average velocities
return {twist.dx / dt, twist.dy / dt, twist.dtheta / dt};
}