mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
[wpimath] Document ChassisSpeeds::Discretize() math (NFC) (#6509)
This commit is contained in:
@@ -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};
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user