[wpimath] Add ChassisSpeeds method to fix drifting during compound swerve drive maneuvers (#5425)

wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h

@@ -4,6 +4,7 @@
 
 #include <numbers>
 
+#include <frc/TimedRobot.h>
 #include <frc/geometry/Translation2d.h>
 #include <frc/kinematics/SwerveDriveKinematics.h>
 #include <frc/trajectory/TrapezoidProfile.h>
@@ -58,6 +59,10 @@ constexpr bool kRearLeftDriveEncoderReversed = true;
 constexpr bool kFrontRightDriveEncoderReversed = false;
 constexpr bool kRearRightDriveEncoderReversed = true;
 
+// If you call DriveSubsystem::Drive with a different period make sure to update
+// this.
+constexpr units::second_t kDrivePeriod = frc::TimedRobot::kDefaultPeriod;
+
 // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
 // These characterization values MUST be determined either experimentally or
 // theoretically for *your* robot's drive. The SysId tool provides a convenient

wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h

@@ -43,7 +43,8 @@ class DriveSubsystem : public frc2::Subsystem {
    */
   void Drive(units::meters_per_second_t xSpeed,
              units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
-             bool fieldRelative);
+             bool fieldRelative,
+             units::second_t period = DriveConstants::kDrivePeriod);
 
   /**
    * Resets the drive encoders to currently read a position of 0.