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

[wpilib] Rename NormalizeWheelSpeeds to DesaturateWheelSpeeds (#3791)

Browse Source
This commit is contained in:
Oblarg
2021-12-30 21:30:08 -05:00
committed by GitHub
parent 102f23bbdb
commit b8d019cdb4
44 changed files with 104 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
View File

@@ -153,7 +153,7 @@ public final class StateSpaceUtil {
}
/**
* Normalize all inputs if any excedes the maximum magnitude. Useful for systems such as
* Renormalize all inputs if any exceeds the maximum magnitude. Useful for systems such as
* differential drivetrains.
*
* @param u The input vector.
@@ -161,7 +161,7 @@ public final class StateSpaceUtil {
* @param <I> The number of inputs.
* @return The normalizedInput
*/
public static <I extends Num> Matrix<I, N1> normalizeInputVector(
public static <I extends Num> Matrix<I, N1> desaturateInputVector(
Matrix<I, N1> u, double maxMagnitude) {
double maxValue = u.maxAbs();
boolean isCapped = maxValue > maxMagnitude;

13
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java
View File

@@ -28,15 +28,16 @@ public class DifferentialDriveWheelSpeeds {
}
/**
* Normalizes the wheel speeds using some max attainable speed. Sometimes, after inverse
* kinematics, the requested speed from a/several modules may be above the max attainable speed
* for the driving motor on that module. To fix this issue, one can "normalize" all the wheel
* speeds to make sure that all requested module speeds are below the absolute threshold, while
* maintaining the ratio of speeds between modules.
* Renormalizes the wheel speeds if any either side is above the specified maximum.
*
* <p>Sometimes, after inverse kinematics, the requested speed from one or more wheels may be
* above the max attainable speed for the driving motor on that wheel. To fix this issue, one can
* reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
* absolute threshold, while maintaining the ratio of speeds between wheels.
*
* @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
*/
public void normalize(double attainableMaxSpeedMetersPerSecond) {
public void desaturate(double attainableMaxSpeedMetersPerSecond) {
double realMaxSpeed = Math.max(Math.abs(leftMetersPerSecond), Math.abs(rightMetersPerSecond));
if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {

2
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
View File

@@ -86,7 +86,7 @@ public class MecanumDriveKinematics {
* component, the robot will rotate around that corner.
* @return The wheel speeds. Use caution because they are not normalized. Sometimes, a user input
* may cause one of the wheel speeds to go above the attainable max velocity. Use the {@link
* MecanumDriveWheelSpeeds#normalize(double)} function to rectify this issue.
* MecanumDriveWheelSpeeds#desaturate(double)} function to rectify this issue.
*/
public MecanumDriveWheelSpeeds toWheelSpeeds(
ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) {

13
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
View File

@@ -43,15 +43,16 @@ public class MecanumDriveWheelSpeeds {
}
/**
* Normalizes the wheel speeds using some max attainable speed. Sometimes, after inverse
* kinematics, the requested speed from a/several modules may be above the max attainable speed
* for the driving motor on that module. To fix this issue, one can "normalize" all the wheel
* speeds to make sure that all requested module speeds are below the absolute threshold, while
* maintaining the ratio of speeds between modules.
* Renormalizes the wheel speeds if any individual speed is above the specified maximum.
*
* <p>Sometimes, after inverse kinematics, the requested speed from one or more wheels may be
* above the max attainable speed for the driving motor on that wheel. To fix this issue, one can
* reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
* absolute threshold, while maintaining the ratio of speeds between wheels.
*
* @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
*/
public void normalize(double attainableMaxSpeedMetersPerSecond) {
public void desaturate(double attainableMaxSpeedMetersPerSecond) {
double realMaxSpeed =
DoubleStream.of(
frontLeftMetersPerSecond,

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

@@ -80,8 +80,8 @@ public class SwerveDriveKinematics {
* component, the robot will rotate around that corner.
* @return An array containing the module states. Use caution because these module states are not
* normalized. Sometimes, a user input may cause one of the module speeds to go above the
* attainable max velocity. Use the {@link #normalizeWheelSpeeds(SwerveModuleState[], double)
* normalizeWheelSpeeds} function to rectify this issue.
* attainable max velocity. Use the {@link #desaturateWheelSpeeds(SwerveModuleState[], double)
* DesaturateWheelSpeeds} function to rectify this issue.
*/
@SuppressWarnings("LocalVariableName")
public SwerveModuleState[] toSwerveModuleStates(
@@ -171,17 +171,18 @@ public class SwerveDriveKinematics {
}
/**
* Normalizes the wheel speeds using some max attainable speed. Sometimes, after inverse
* kinematics, the requested speed from a/several modules may be above the max attainable speed
* for the driving motor on that module. To fix this issue, one can "normalize" all the wheel
* speeds to make sure that all requested module speeds are below the absolute threshold, while
* maintaining the ratio of speeds between modules.
* Renormalizes the wheel speeds if any individual speed is above the specified maximum.
*
* <p>Sometimes, after inverse kinematics, the requested speed from one or more modules may be
* above the max attainable speed for the driving motor on that module. To fix this issue, one can
* reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
* absolute threshold, while maintaining the ratio of speeds between modules.
*
* @param moduleStates Reference to array of module states. The array will be mutated with the
* normalized speeds!
* @param attainableMaxSpeedMetersPerSecond The absolute max speed that a module can reach.
*/
public static void normalizeWheelSpeeds(
public static void desaturateWheelSpeeds(
SwerveModuleState[] moduleStates, double attainableMaxSpeedMetersPerSecond) {
double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {

4
wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java
View File

@@ -57,7 +57,7 @@ public class LinearSystemLoop<States extends Num, Inputs extends Num, Outputs ex
controller,
new LinearPlantInversionFeedforward<>(plant, dtSeconds),
observer,
u -> StateSpaceUtil.normalizeInputVector(u, maxVoltageVolts));
u -> StateSpaceUtil.desaturateInputVector(u, maxVoltageVolts));
}
/**
@@ -104,7 +104,7 @@ public class LinearSystemLoop<States extends Num, Inputs extends Num, Outputs ex
controller,
feedforward,
observer,
u -> StateSpaceUtil.normalizeInputVector(u, maxVoltageVolts));
u -> StateSpaceUtil.desaturateInputVector(u, maxVoltageVolts));
}
/**

2
wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
View File

@@ -48,7 +48,7 @@ public class DifferentialDriveKinematicsConstraint implements TrajectoryConstrai
// Get the wheel speeds and normalize them to within the max velocity.
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
wheelSpeeds.normalize(m_maxSpeedMetersPerSecond);
wheelSpeeds.desaturate(m_maxSpeedMetersPerSecond);
// Return the new linear chassis speed.
return m_kinematics.toChassisSpeeds(wheelSpeeds).vxMetersPerSecond;

2
wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java
View File

@@ -53,7 +53,7 @@ public class MecanumDriveKinematicsConstraint implements TrajectoryConstraint {
// Get the wheel speeds and normalize them to within the max velocity.
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
wheelSpeeds.normalize(m_maxSpeedMetersPerSecond);
wheelSpeeds.desaturate(m_maxSpeedMetersPerSecond);
// Convert normalized wheel speeds back to chassis speeds
var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);

2
wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java
View File

@@ -53,7 +53,7 @@ public class SwerveDriveKinematicsConstraint implements TrajectoryConstraint {
// Get the wheel speeds and normalize them to within the max velocity.
var wheelSpeeds = m_kinematics.toSwerveModuleStates(chassisSpeeds);
SwerveDriveKinematics.normalizeWheelSpeeds(wheelSpeeds, m_maxSpeedMetersPerSecond);
SwerveDriveKinematics.desaturateWheelSpeeds(wheelSpeeds, m_maxSpeedMetersPerSecond);
// Convert normalized wheel speeds back to chassis speeds
var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);