diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java index 0b851e3a91..e551c932dc 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java @@ -302,7 +302,7 @@ public class SwerveDriveKinematics * * @param moduleStates Reference to array of module states. The array will be mutated with the * normalized speeds! - * @param currentChassisSpeed The current speed of the robot + * @param desiredChassisSpeed The desired speed of the robot * @param attainableMaxModuleSpeedMetersPerSecond The absolute max speed that a module can reach * @param attainableMaxTranslationalSpeedMetersPerSecond The absolute max speed that your robot * can reach while translating @@ -311,7 +311,7 @@ public class SwerveDriveKinematics */ public static void desaturateWheelSpeeds( SwerveModuleState[] moduleStates, - ChassisSpeeds currentChassisSpeed, + ChassisSpeeds desiredChassisSpeed, double attainableMaxModuleSpeedMetersPerSecond, double attainableMaxTranslationalSpeedMetersPerSecond, double attainableMaxRotationalVelocityRadiansPerSecond) { @@ -326,10 +326,10 @@ public class SwerveDriveKinematics return; } double translationalK = - Math.hypot(currentChassisSpeed.vxMetersPerSecond, currentChassisSpeed.vyMetersPerSecond) + Math.hypot(desiredChassisSpeed.vxMetersPerSecond, desiredChassisSpeed.vyMetersPerSecond) / attainableMaxTranslationalSpeedMetersPerSecond; double rotationalK = - Math.abs(currentChassisSpeed.omegaRadiansPerSecond) + Math.abs(desiredChassisSpeed.omegaRadiansPerSecond) / attainableMaxRotationalVelocityRadiansPerSecond; double k = Math.max(translationalK, rotationalK); double scale = Math.min(k * attainableMaxModuleSpeedMetersPerSecond / realMaxSpeed, 1); diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index 63fe4c90b0..eee14af453 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -271,7 +271,7 @@ class SwerveDriveKinematics * * @param moduleStates Reference to array of module states. The array will be * mutated with the normalized speeds! - * @param currentChassisSpeed Current speed of the robot + * @param desiredChassisSpeed The desired speed of the robot * @param attainableMaxModuleSpeed The absolute max speed a module can reach * @param attainableMaxRobotTranslationSpeed The absolute max speed the robot * can reach while translating @@ -280,7 +280,7 @@ class SwerveDriveKinematics */ static void DesaturateWheelSpeeds( wpi::array* moduleStates, - ChassisSpeeds currentChassisSpeed, + ChassisSpeeds desiredChassisSpeed, units::meters_per_second_t attainableMaxModuleSpeed, units::meters_per_second_t attainableMaxRobotTranslationSpeed, units::radians_per_second_t attainableMaxRobotRotationSpeed); diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc index d29fad7c02..db85d999e3 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc @@ -137,7 +137,7 @@ void SwerveDriveKinematics::DesaturateWheelSpeeds( template void SwerveDriveKinematics::DesaturateWheelSpeeds( wpi::array* moduleStates, - ChassisSpeeds currentChassisSpeed, + ChassisSpeeds desiredChassisSpeed, units::meters_per_second_t attainableMaxModuleSpeed, units::meters_per_second_t attainableMaxRobotTranslationSpeed, units::radians_per_second_t attainableMaxRobotRotationSpeed) { @@ -157,10 +157,10 @@ void SwerveDriveKinematics::DesaturateWheelSpeeds( } auto translationalK = - units::math::hypot(currentChassisSpeed.vx, currentChassisSpeed.vy) / + units::math::hypot(desiredChassisSpeed.vx, desiredChassisSpeed.vy) / attainableMaxRobotTranslationSpeed; - auto rotationalK = units::math::abs(currentChassisSpeed.omega) / + auto rotationalK = units::math::abs(desiredChassisSpeed.omega) / attainableMaxRobotRotationSpeed; auto k = units::math::max(translationalK, rotationalK);