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

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

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files

7
wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
View File

@@ -99,12 +99,19 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
double vyMetersPerSecond,
double omegaRadiansPerSecond,
double dtSeconds) {
// Construct the desired pose after a timestep, relative to the current pose. The desired pose
// has decoupled translation and rotation.
var desiredDeltaPose =
new Pose2d(
vxMetersPerSecond * dtSeconds,
vyMetersPerSecond * dtSeconds,
new Rotation2d(omegaRadiansPerSecond * dtSeconds));
// Find the chassis translation/rotation deltas in the robot frame that move the robot from its
// current pose to the desired pose
var twist = new Pose2d().log(desiredDeltaPose);
// Turn the chassis translation/rotation deltas into average velocities
return new ChassisSpeeds(twist.dx / dtSeconds, twist.dy / dtSeconds, twist.dtheta / dtSeconds);
}