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

[wpimath] SwerveDriveKinematics: Rename currentChassisSpeed to desiredChassisSpeed (#5393)

Browse Source
This commit is contained in:
Gold856
2023-06-20 01:58:38 -04:00
committed by GitHub
parent 50b90ceb54
commit 5fc4aee2d2
3 changed files with 9 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
View File

@@ -302,7 +302,7 @@ public class SwerveDriveKinematics
* *
* @param moduleStates Reference to array of module states. The array will be mutated with the * @param moduleStates Reference to array of module states. The array will be mutated with the
* normalized speeds! * 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 attainableMaxModuleSpeedMetersPerSecond The absolute max speed that a module can reach
* @param attainableMaxTranslationalSpeedMetersPerSecond The absolute max speed that your robot * @param attainableMaxTranslationalSpeedMetersPerSecond The absolute max speed that your robot
* can reach while translating * can reach while translating
@@ -311,7 +311,7 @@ public class SwerveDriveKinematics
*/ */
public static void desaturateWheelSpeeds( public static void desaturateWheelSpeeds(
SwerveModuleState[] moduleStates, SwerveModuleState[] moduleStates,
ChassisSpeeds currentChassisSpeed, ChassisSpeeds desiredChassisSpeed,
double attainableMaxModuleSpeedMetersPerSecond, double attainableMaxModuleSpeedMetersPerSecond,
double attainableMaxTranslationalSpeedMetersPerSecond, double attainableMaxTranslationalSpeedMetersPerSecond,
double attainableMaxRotationalVelocityRadiansPerSecond) { double attainableMaxRotationalVelocityRadiansPerSecond) {
@@ -326,10 +326,10 @@ public class SwerveDriveKinematics
return; return;
} }
double translationalK = double translationalK =
Math.hypot(currentChassisSpeed.vxMetersPerSecond, currentChassisSpeed.vyMetersPerSecond) Math.hypot(desiredChassisSpeed.vxMetersPerSecond, desiredChassisSpeed.vyMetersPerSecond)
/ attainableMaxTranslationalSpeedMetersPerSecond; / attainableMaxTranslationalSpeedMetersPerSecond;
double rotationalK = double rotationalK =
Math.abs(currentChassisSpeed.omegaRadiansPerSecond) Math.abs(desiredChassisSpeed.omegaRadiansPerSecond)
/ attainableMaxRotationalVelocityRadiansPerSecond; / attainableMaxRotationalVelocityRadiansPerSecond;
double k = Math.max(translationalK, rotationalK); double k = Math.max(translationalK, rotationalK);
double scale = Math.min(k * attainableMaxModuleSpeedMetersPerSecond / realMaxSpeed, 1); double scale = Math.min(k * attainableMaxModuleSpeedMetersPerSecond / realMaxSpeed, 1);

4
wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
View File

@@ -271,7 +271,7 @@ class SwerveDriveKinematics
* *
* @param moduleStates Reference to array of module states. The array will be * @param moduleStates Reference to array of module states. The array will be
* mutated with the normalized speeds! * 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 attainableMaxModuleSpeed The absolute max speed a module can reach
* @param attainableMaxRobotTranslationSpeed The absolute max speed the robot * @param attainableMaxRobotTranslationSpeed The absolute max speed the robot
* can reach while translating * can reach while translating
@@ -280,7 +280,7 @@ class SwerveDriveKinematics
*/ */
static void DesaturateWheelSpeeds( static void DesaturateWheelSpeeds(
wpi::array<SwerveModuleState, NumModules>* moduleStates, wpi::array<SwerveModuleState, NumModules>* moduleStates,
ChassisSpeeds currentChassisSpeed, ChassisSpeeds desiredChassisSpeed,
units::meters_per_second_t attainableMaxModuleSpeed, units::meters_per_second_t attainableMaxModuleSpeed,
units::meters_per_second_t attainableMaxRobotTranslationSpeed, units::meters_per_second_t attainableMaxRobotTranslationSpeed,
units::radians_per_second_t attainableMaxRobotRotationSpeed); units::radians_per_second_t attainableMaxRobotRotationSpeed);

6
wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
View File

@@ -137,7 +137,7 @@ void SwerveDriveKinematics<NumModules>::DesaturateWheelSpeeds(
template <size_t NumModules> template <size_t NumModules>
void SwerveDriveKinematics<NumModules>::DesaturateWheelSpeeds( void SwerveDriveKinematics<NumModules>::DesaturateWheelSpeeds(
wpi::array<SwerveModuleState, NumModules>* moduleStates, wpi::array<SwerveModuleState, NumModules>* moduleStates,
ChassisSpeeds currentChassisSpeed, ChassisSpeeds desiredChassisSpeed,
units::meters_per_second_t attainableMaxModuleSpeed, units::meters_per_second_t attainableMaxModuleSpeed,
units::meters_per_second_t attainableMaxRobotTranslationSpeed, units::meters_per_second_t attainableMaxRobotTranslationSpeed,
units::radians_per_second_t attainableMaxRobotRotationSpeed) { units::radians_per_second_t attainableMaxRobotRotationSpeed) {
@@ -157,10 +157,10 @@ void SwerveDriveKinematics<NumModules>::DesaturateWheelSpeeds(
} }
auto translationalK = auto translationalK =
units::math::hypot(currentChassisSpeed.vx, currentChassisSpeed.vy) / units::math::hypot(desiredChassisSpeed.vx, desiredChassisSpeed.vy) /
attainableMaxRobotTranslationSpeed; attainableMaxRobotTranslationSpeed;
auto rotationalK = units::math::abs(currentChassisSpeed.omega) / auto rotationalK = units::math::abs(desiredChassisSpeed.omega) /
attainableMaxRobotRotationSpeed; attainableMaxRobotRotationSpeed;
auto k = units::math::max(translationalK, rotationalK); auto k = units::math::max(translationalK, rotationalK);