Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41: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
* 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);

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
* 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<SwerveModuleState, NumModules>* moduleStates,
ChassisSpeeds currentChassisSpeed,
ChassisSpeeds desiredChassisSpeed,
units::meters_per_second_t attainableMaxModuleSpeed,
units::meters_per_second_t attainableMaxRobotTranslationSpeed,
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>
void SwerveDriveKinematics<NumModules>::DesaturateWheelSpeeds(
wpi::array<SwerveModuleState, NumModules>* 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<NumModules>::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);