Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-29 02:21:44 +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

2
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
View File

@@ -359,7 +359,7 @@ public class MecanumControllerCommand extends CommandBase {
m_controller.calculate(m_pose.get(), desiredState, m_desiredRotation.get());
var targetWheelSpeeds = m_kinematics.toWheelSpeeds(targetChassisSpeeds);
targetWheelSpeeds.normalize(m_maxWheelVelocityMetersPerSecond);
targetWheelSpeeds.desaturate(m_maxWheelVelocityMetersPerSecond);
var frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond;
var rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond;

2
wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
View File

@@ -278,7 +278,7 @@ void MecanumControllerCommand::Execute() {
m_controller.Calculate(m_pose(), m_desiredState, m_desiredRotation());
auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(targetChassisSpeeds);
targetWheelSpeeds.Normalize(m_maxWheelVelocity);
targetWheelSpeeds.Desaturate(m_maxWheelVelocity);
auto frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeft;
auto rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeft;

2
wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
View File

@@ -102,7 +102,7 @@ KilloughDrive::WheelSpeeds KilloughDrive::DriveCartesianIK(double ySpeed,
wheelSpeeds[kRight] = input.ScalarProject(m_rightVec) + zRotation;
wheelSpeeds[kBack] = input.ScalarProject(m_backVec) + zRotation;
Normalize(wheelSpeeds);
Desaturate(wheelSpeeds);
return {wheelSpeeds[kLeft], wheelSpeeds[kRight], wheelSpeeds[kBack]};
}

2
wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
View File

@@ -103,7 +103,7 @@ MecanumDrive::WheelSpeeds MecanumDrive::DriveCartesianIK(double ySpeed,
wheelSpeeds[kRearLeft] = input.x - input.y + zRotation;
wheelSpeeds[kRearRight] = input.x + input.y - zRotation;
Normalize(wheelSpeeds);
Desaturate(wheelSpeeds);
return {wheelSpeeds[kFrontLeft], wheelSpeeds[kFrontRight],
wheelSpeeds[kRearLeft], wheelSpeeds[kRearRight]};

2
wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
View File

@@ -35,7 +35,7 @@ double RobotDriveBase::ApplyDeadband(double value, double deadband) {
return frc::ApplyDeadband(value, deadband);
}
void RobotDriveBase::Normalize(wpi::span<double> wheelSpeeds) {
void RobotDriveBase::Desaturate(wpi::span<double> wheelSpeeds) {
double maxMagnitude = std::abs(wheelSpeeds[0]);
for (size_t i = 1; i < wheelSpeeds.size(); i++) {
double temp = std::abs(wheelSpeeds[i]);

4
wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
View File

@@ -43,8 +43,8 @@ DifferentialDrivetrainSim::DifferentialDrivetrainSim(
Eigen::Vector<double, 2> DifferentialDrivetrainSim::ClampInput(
const Eigen::Vector<double, 2>& u) {
return frc::NormalizeInputVector<2>(u,
frc::RobotController::GetInputVoltage());
return frc::DesaturateInputVector<2>(u,
frc::RobotController::GetInputVoltage());
}
void DifferentialDrivetrainSim::SetInputs(units::volt_t leftVoltage,

4
wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h
View File

@@ -82,10 +82,10 @@ class RobotDriveBase : public MotorSafety {
static double ApplyDeadband(double value, double deadband);
/**
* Normalize all wheel speeds if the magnitude of any wheel is greater than
* Renormalize all wheel speeds if the magnitude of any wheel is greater than
* 1.0.
*/
static void Normalize(wpi::span<double> wheelSpeeds);
static void Desaturate(wpi::span<double> wheelSpeeds);
double m_deadband = 0.02;
double m_maxOutput = 1.0;

2
wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
View File

@@ -136,7 +136,7 @@ class LinearSystemSim {
* @return The normalized input.
*/
Eigen::Vector<double, Inputs> ClampInput(Eigen::Vector<double, Inputs> u) {
return frc::NormalizeInputVector<Inputs>(
return frc::DesaturateInputVector<Inputs>(
u, frc::RobotController::GetInputVoltage());
}

2
wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp
View File

@@ -47,7 +47,7 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot});
wheelSpeeds.Normalize(kMaxSpeed);
wheelSpeeds.Desaturate(kMaxSpeed);
SetSpeeds(wheelSpeeds);
}

2
wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp
View File

@@ -44,7 +44,7 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot});
wheelSpeeds.Normalize(kMaxSpeed);
wheelSpeeds.Desaturate(kMaxSpeed);
SetSpeeds(wheelSpeeds);
}

2
wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp
View File

@@ -12,7 +12,7 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot});
m_kinematics.NormalizeWheelSpeeds(&states, kMaxSpeed);
m_kinematics.DesaturateWheelSpeeds(&states, kMaxSpeed);
auto [fl, fr, bl, br] = states;

6
wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -54,7 +54,7 @@ void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot});
kDriveKinematics.NormalizeWheelSpeeds(&states, AutoConstants::kMaxSpeed);
kDriveKinematics.DesaturateWheelSpeeds(&states, AutoConstants::kMaxSpeed);
auto [fl, fr, bl, br] = states;
@@ -66,8 +66,8 @@ void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,
void DriveSubsystem::SetModuleStates(
wpi::array<frc::SwerveModuleState, 4> desiredStates) {
kDriveKinematics.NormalizeWheelSpeeds(&desiredStates,
AutoConstants::kMaxSpeed);
kDriveKinematics.DesaturateWheelSpeeds(&desiredStates,
AutoConstants::kMaxSpeed);
m_frontLeft.SetDesiredState(desiredStates[0]);
m_frontRight.SetDesiredState(desiredStates[1]);
m_rearLeft.SetDesiredState(desiredStates[2]);

2
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp
View File

@@ -16,7 +16,7 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot});
m_kinematics.NormalizeWheelSpeeds(&states, kMaxSpeed);
m_kinematics.DesaturateWheelSpeeds(&states, kMaxSpeed);
auto [fl, fr, bl, br] = states;

2
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
View File

@@ -360,7 +360,7 @@ public class DifferentialDrivetrainSim {
* @return The normalized input.
*/
protected Matrix<N2, N1> clampInput(Matrix<N2, N1> u) {
return StateSpaceUtil.normalizeInputVector(u, RobotController.getBatteryVoltage());
return StateSpaceUtil.desaturateInputVector(u, RobotController.getBatteryVoltage());
}
/** Represents the different states of the drivetrain. */

2
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java
View File

@@ -186,6 +186,6 @@ public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs ext
* @return The normalized input.
*/
protected Matrix<Inputs, N1> clampInput(Matrix<Inputs, N1> u) {
return StateSpaceUtil.normalizeInputVector(u, RobotController.getBatteryVoltage());
return StateSpaceUtil.desaturateInputVector(u, RobotController.getBatteryVoltage());
}
}

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
View File

@@ -121,7 +121,7 @@ public class Drivetrain {
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeed, ySpeed, rot));
mecanumDriveWheelSpeeds.normalize(kMaxSpeed);
mecanumDriveWheelSpeeds.desaturate(kMaxSpeed);
setSpeeds(mecanumDriveWheelSpeeds);
}

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
View File

@@ -133,7 +133,7 @@ public class Drivetrain {
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeed, ySpeed, rot));
mecanumDriveWheelSpeeds.normalize(kMaxSpeed);
mecanumDriveWheelSpeeds.desaturate(kMaxSpeed);
setSpeeds(mecanumDriveWheelSpeeds);
}

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
View File

@@ -53,7 +53,7 @@ public class Drivetrain {
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeed, ySpeed, rot));
SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates, kMaxSpeed);
SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed);
m_frontLeft.setDesiredState(swerveModuleStates[0]);
m_frontRight.setDesiredState(swerveModuleStates[1]);
m_backLeft.setDesiredState(swerveModuleStates[2]);

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
View File

@@ -106,7 +106,7 @@ public class DriveSubsystem extends SubsystemBase {
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeed, ySpeed, rot));
SwerveDriveKinematics.normalizeWheelSpeeds(
SwerveDriveKinematics.desaturateWheelSpeeds(
swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond);
m_frontLeft.setDesiredState(swerveModuleStates[0]);
m_frontRight.setDesiredState(swerveModuleStates[1]);
@@ -120,7 +120,7 @@ public class DriveSubsystem extends SubsystemBase {
* @param desiredStates The desired SwerveModule states.
*/
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.normalizeWheelSpeeds(
SwerveDriveKinematics.desaturateWheelSpeeds(
desiredStates, DriveConstants.kMaxSpeedMetersPerSecond);
m_frontLeft.setDesiredState(desiredStates[0]);
m_frontRight.setDesiredState(desiredStates[1]);

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
View File

@@ -65,7 +65,7 @@ public class Drivetrain {
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeed, ySpeed, rot));
SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates, kMaxSpeed);
SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed);
m_frontLeft.setDesiredState(swerveModuleStates[0]);
m_frontRight.setDesiredState(swerveModuleStates[1]);
m_backLeft.setDesiredState(swerveModuleStates[2]);

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);

2
wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp
View File

@@ -8,7 +8,7 @@
using namespace frc;
void DifferentialDriveWheelSpeeds::Normalize(
void DifferentialDriveWheelSpeeds::Desaturate(
units::meters_per_second_t attainableMaxSpeed) {
auto realMaxSpeed =
units::math::max(units::math::abs(left), units::math::abs(right));

2
wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
View File

@@ -12,7 +12,7 @@
using namespace frc;
void MecanumDriveWheelSpeeds::Normalize(
void MecanumDriveWheelSpeeds::Desaturate(
units::meters_per_second_t attainableMaxSpeed) {
std::array<units::meters_per_second_t, 4> wheelSpeeds{frontLeft, frontRight,
rearLeft, rearRight};

2
wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
View File

@@ -16,7 +16,7 @@ units::meters_per_second_t DifferentialDriveKinematicsConstraint::MaxVelocity(
units::meters_per_second_t velocity) const {
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds({velocity, 0_mps, velocity * curvature});
wheelSpeeds.Normalize(m_maxSpeed);
wheelSpeeds.Desaturate(m_maxSpeed);
return m_kinematics.ToChassisSpeeds(wheelSpeeds).vx;
}

2
wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
View File

@@ -20,7 +20,7 @@ units::meters_per_second_t MecanumDriveKinematicsConstraint::MaxVelocity(
auto yVelocity = velocity * pose.Rotation().Sin();
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds({xVelocity, yVelocity, velocity * curvature});
wheelSpeeds.Normalize(m_maxSpeed);
wheelSpeeds.Desaturate(m_maxSpeed);
auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);

6
wpimath/src/main/native/include/frc/StateSpaceUtil.h
View File

@@ -336,8 +336,8 @@ Eigen::Vector<double, Inputs> ClampInputMaxMagnitude(
}
/**
* Normalize all inputs if any excedes the maximum magnitude. Useful for systems
* such as differential drivetrains.
* Renormalize all inputs if any exceeds the maximum magnitude. Useful for
* systems such as differential drivetrains.
*
* @tparam Inputs The number of inputs.
* @param u The input vector.
@@ -345,7 +345,7 @@ Eigen::Vector<double, Inputs> ClampInputMaxMagnitude(
* @return The normalizedInput
*/
template <int Inputs>
Eigen::Vector<double, Inputs> NormalizeInputVector(
Eigen::Vector<double, Inputs> DesaturateInputVector(
const Eigen::Vector<double, Inputs>& u, double maxMagnitude) {
double maxValue = u.template lpNorm<Eigen::Infinity>();

16
wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
View File

@@ -24,15 +24,17 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelSpeeds {
units::meters_per_second_t right = 0_mps;
/**
* 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 either side is above the specified
* maximum.
*
* 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 attainableMaxSpeed The absolute max speed that a wheel can reach.
*/
void Normalize(units::meters_per_second_t attainableMaxSpeed);
void Desaturate(units::meters_per_second_t attainableMaxSpeed);
};
} // namespace frc

16
wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
View File

@@ -34,15 +34,17 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelSpeeds {
units::meters_per_second_t rearRight = 0_mps;
/**
* 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.
*
* 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 attainableMaxSpeed The absolute max speed that a wheel can reach.
*/
void Normalize(units::meters_per_second_t attainableMaxSpeed);
void Desaturate(units::meters_per_second_t attainableMaxSpeed);
};
} // namespace frc

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

@@ -114,7 +114,7 @@ class SwerveDriveKinematics {
* @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
* NormalizeWheelSpeeds(wpi::array<SwerveModuleState, NumModules>*,
* DesaturateWheelSpeeds(wpi::array<SwerveModuleState, NumModules>*,
* units::meters_per_second_t) function to rectify this issue. In addition,
* you can leverage the power of C++17 to directly assign the module states to
* variables:
@@ -159,18 +159,21 @@ class SwerveDriveKinematics {
wpi::array<SwerveModuleState, NumModules> moduleStates) const;
/**
* 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.
*
* 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 attainableMaxSpeed The absolute max speed that a module can reach.
*/
static void NormalizeWheelSpeeds(
static void DesaturateWheelSpeeds(
wpi::array<SwerveModuleState, NumModules>* moduleStates,
units::meters_per_second_t attainableMaxSpeed);

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

@@ -88,7 +88,7 @@ ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
}
template <size_t NumModules>
void SwerveDriveKinematics<NumModules>::NormalizeWheelSpeeds(
void SwerveDriveKinematics<NumModules>::DesaturateWheelSpeeds(
wpi::array<SwerveModuleState, NumModules>* moduleStates,
units::meters_per_second_t attainableMaxSpeed) {
auto& states = *moduleStates;

4
wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
View File

@@ -54,7 +54,7 @@ class LinearSystemLoop {
: LinearSystemLoop(
plant, controller, observer,
[=](const Eigen::Vector<double, Inputs>& u) {
return frc::NormalizeInputVector<Inputs>(u, maxVoltage.value());
return frc::DesaturateInputVector<Inputs>(u, maxVoltage.value());
},
dt) {}
@@ -99,7 +99,7 @@ class LinearSystemLoop {
KalmanFilter<States, Inputs, Outputs>& observer, units::volt_t maxVoltage)
: LinearSystemLoop(controller, feedforward, observer,
[=](const Eigen::Vector<double, Inputs>& u) {
return frc::NormalizeInputVector<Inputs>(
return frc::DesaturateInputVector<Inputs>(
u, maxVoltage.value());
}) {}

2
wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc
View File

@@ -24,7 +24,7 @@ SwerveDriveKinematicsConstraint<NumModules>::MaxVelocity(
auto yVelocity = velocity * pose.Rotation().Sin();
auto wheelSpeeds = m_kinematics.ToSwerveModuleStates(
{xVelocity, yVelocity, velocity * curvature});
m_kinematics.NormalizeWheelSpeeds(&wheelSpeeds, m_maxSpeed);
m_kinematics.DesaturateWheelSpeeds(&wheelSpeeds, m_maxSpeed);
auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);

4
wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java
View File

@@ -160,9 +160,9 @@ class MecanumDriveKinematicsTest {
}
@Test
void testNormalize() {
void testDesaturate() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(5, 6, 4, 7);
wheelSpeeds.normalize(5.5);
wheelSpeeds.desaturate(5.5);
double factor = 5.5 / 7.0;

4
wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
View File

@@ -229,14 +229,14 @@ class SwerveDriveKinematicsTest {
}
@Test
void testNormalize() {
void testDesaturate() {
SwerveModuleState fl = new SwerveModuleState(5, new Rotation2d());
SwerveModuleState fr = new SwerveModuleState(6, new Rotation2d());
SwerveModuleState bl = new SwerveModuleState(4, new Rotation2d());
SwerveModuleState br = new SwerveModuleState(7, new Rotation2d());
SwerveModuleState[] arr = {fl, fr, bl, br};
SwerveDriveKinematics.normalizeWheelSpeeds(arr, 5.5);
SwerveDriveKinematics.desaturateWheelSpeeds(arr, 5.5);
double factor = 5.5 / 7.0;

4
wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
View File

@@ -143,9 +143,9 @@ TEST_F(MecanumDriveKinematicsTest,
EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, Normalize) {
TEST_F(MecanumDriveKinematicsTest, Desaturate) {
MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps};
wheelSpeeds.Normalize(5.5_mps);
wheelSpeeds.Desaturate(5.5_mps);
double kFactor = 5.5 / 7.0;

4
wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
View File

@@ -162,14 +162,14 @@ TEST_F(SwerveDriveKinematicsTest,
EXPECT_NEAR(chassisSpeeds.omega.value(), 1.5, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, Normalize) {
TEST_F(SwerveDriveKinematicsTest, Desaturate) {
SwerveModuleState state1{5.0_mps, Rotation2d()};
SwerveModuleState state2{6.0_mps, Rotation2d()};
SwerveModuleState state3{4.0_mps, Rotation2d()};
SwerveModuleState state4{7.0_mps, Rotation2d()};
wpi::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::NormalizeWheelSpeeds(&arr, 5.5_mps);
SwerveDriveKinematics<4>::DesaturateWheelSpeeds(&arr, 5.5_mps);
double kFactor = 5.5 / 7.0;