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

Author: calcmogul

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);
   }
 

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};
   }