From 2def62a1ef8f94c5caea9e1f02de0f417cf6cb7e Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Wed, 10 Apr 2024 22:03:44 -0700
Subject: [PATCH] [wpimath] Document ChassisSpeeds::Discretize() math (NFC)
 (#6509)

---
 .../java/edu/wpi/first/math/kinematics/ChassisSpeeds.java  | 7 +++++++
 .../src/main/native/include/frc/kinematics/ChassisSpeeds.h | 7 +++++++
 2 files changed, 14 insertions(+)

diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
index 33a5edca43e..4e1a8c9355b 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
@@ -99,12 +99,19 @@ public static ChassisSpeeds discretize(
       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);
   }
 
diff --git a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
index ac117b63e38..f8e84465882 100644
--- a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
+++ b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
@@ -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};
   }