From b8d019cdb47ce4b091b92f91637d512b7d800f26 Mon Sep 17 00:00:00 2001 From: Oblarg Date: Thu, 30 Dec 2021 21:30:08 -0500 Subject: [PATCH] [wpilib] Rename NormalizeWheelSpeeds to DesaturateWheelSpeeds (#3791) --- .../command/MecanumControllerCommand.java | 2 +- .../frc2/command/MecanumControllerCommand.cpp | 2 +- .../main/native/cpp/drive/KilloughDrive.cpp | 2 +- .../main/native/cpp/drive/MecanumDrive.cpp | 2 +- .../main/native/cpp/drive/RobotDriveBase.cpp | 2 +- .../simulation/DifferentialDrivetrainSim.cpp | 4 ++-- .../native/include/frc/drive/RobotDriveBase.h | 4 ++-- .../include/frc/simulation/LinearSystemSim.h | 2 +- .../examples/MecanumBot/cpp/Drivetrain.cpp | 2 +- .../cpp/Drivetrain.cpp | 2 +- .../cpp/examples/SwerveBot/cpp/Drivetrain.cpp | 2 +- .../cpp/subsystems/DriveSubsystem.cpp | 6 +++--- .../cpp/Drivetrain.cpp | 2 +- .../simulation/DifferentialDrivetrainSim.java | 2 +- .../wpilibj/simulation/LinearSystemSim.java | 2 +- .../examples/mecanumbot/Drivetrain.java | 2 +- .../mecanumdriveposeestimator/Drivetrain.java | 2 +- .../examples/swervebot/Drivetrain.java | 2 +- .../subsystems/DriveSubsystem.java | 4 ++-- .../swervedriveposeestimator/Drivetrain.java | 2 +- .../edu/wpi/first/math/StateSpaceUtil.java | 4 ++-- .../DifferentialDriveWheelSpeeds.java | 13 +++++++------ .../kinematics/MecanumDriveKinematics.java | 2 +- .../kinematics/MecanumDriveWheelSpeeds.java | 13 +++++++------ .../kinematics/SwerveDriveKinematics.java | 17 +++++++++-------- .../first/math/system/LinearSystemLoop.java | 4 ++-- ...DifferentialDriveKinematicsConstraint.java | 2 +- .../MecanumDriveKinematicsConstraint.java | 2 +- .../SwerveDriveKinematicsConstraint.java | 2 +- .../DifferentialDriveWheelSpeeds.cpp | 2 +- .../kinematics/MecanumDriveWheelSpeeds.cpp | 2 +- .../DifferentialDriveKinematicsConstraint.cpp | 2 +- .../MecanumDriveKinematicsConstraint.cpp | 2 +- .../main/native/include/frc/StateSpaceUtil.h | 6 +++--- .../kinematics/DifferentialDriveWheelSpeeds.h | 16 +++++++++------- .../frc/kinematics/MecanumDriveWheelSpeeds.h | 16 +++++++++------- .../frc/kinematics/SwerveDriveKinematics.h | 19 +++++++++++-------- .../frc/kinematics/SwerveDriveKinematics.inc | 2 +- .../include/frc/system/LinearSystemLoop.h | 4 ++-- .../SwerveDriveKinematicsConstraint.inc | 2 +- .../MecanumDriveKinematicsTest.java | 4 ++-- .../kinematics/SwerveDriveKinematicsTest.java | 4 ++-- .../kinematics/MecanumDriveKinematicsTest.cpp | 4 ++-- .../kinematics/SwerveDriveKinematicsTest.cpp | 4 ++-- 44 files changed, 104 insertions(+), 94 deletions(-) diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java index 28c75a9b1e..c22259c256 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java @@ -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; diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp index cfa3a6d308..14b5846d68 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp @@ -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; diff --git a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp index c84767801b..268ae64871 100644 --- a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp +++ b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp @@ -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]}; } diff --git a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp index b74c6113a0..70e90d4b5b 100644 --- a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp +++ b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp @@ -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]}; diff --git a/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp b/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp index 215995840e..97ea0ee55f 100644 --- a/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp +++ b/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp @@ -35,7 +35,7 @@ double RobotDriveBase::ApplyDeadband(double value, double deadband) { return frc::ApplyDeadband(value, deadband); } -void RobotDriveBase::Normalize(wpi::span wheelSpeeds) { +void RobotDriveBase::Desaturate(wpi::span wheelSpeeds) { double maxMagnitude = std::abs(wheelSpeeds[0]); for (size_t i = 1; i < wheelSpeeds.size(); i++) { double temp = std::abs(wheelSpeeds[i]); diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp index 0bbf9c381c..78b9dc79b6 100644 --- a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp @@ -43,8 +43,8 @@ DifferentialDrivetrainSim::DifferentialDrivetrainSim( Eigen::Vector DifferentialDrivetrainSim::ClampInput( const Eigen::Vector& u) { - return frc::NormalizeInputVector<2>(u, - frc::RobotController::GetInputVoltage()); + return frc::DesaturateInputVector<2>(u, + frc::RobotController::GetInputVoltage()); } void DifferentialDrivetrainSim::SetInputs(units::volt_t leftVoltage, diff --git a/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h b/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h index 389d67b370..8ce5636709 100644 --- a/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h +++ b/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h @@ -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 wheelSpeeds); + static void Desaturate(wpi::span wheelSpeeds); double m_deadband = 0.02; double m_maxOutput = 1.0; diff --git a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h index 61cbd4630b..9e507a89ba 100644 --- a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h @@ -136,7 +136,7 @@ class LinearSystemSim { * @return The normalized input. */ Eigen::Vector ClampInput(Eigen::Vector u) { - return frc::NormalizeInputVector( + return frc::DesaturateInputVector( u, frc::RobotController::GetInputVoltage()); } diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp index c33450d0cd..4d3aac221d 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp @@ -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); } diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp index 149f12e3fa..daa4b42307 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp @@ -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); } diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp index 7044fd77e9..709f40314e 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp @@ -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; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp index 60f619e2d6..fd15100847 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp @@ -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 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]); diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp index f132f76b17..33a3233701 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp @@ -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; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java index 9bb0a2f121..43242d1bba 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java @@ -360,7 +360,7 @@ public class DifferentialDrivetrainSim { * @return The normalized input. */ protected Matrix clampInput(Matrix u) { - return StateSpaceUtil.normalizeInputVector(u, RobotController.getBatteryVoltage()); + return StateSpaceUtil.desaturateInputVector(u, RobotController.getBatteryVoltage()); } /** Represents the different states of the drivetrain. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java index 2b4e9a978d..e6ffbca056 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java @@ -186,6 +186,6 @@ public class LinearSystemSim clampInput(Matrix u) { - return StateSpaceUtil.normalizeInputVector(u, RobotController.getBatteryVoltage()); + return StateSpaceUtil.desaturateInputVector(u, RobotController.getBatteryVoltage()); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java index 7ed24a79b3..9210265673 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java @@ -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); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java index fa2813f784..427284f918 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java @@ -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); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java index 3575266217..3b71a0c644 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java @@ -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]); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java index 64d65b3e6f..efd35528c6 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java @@ -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]); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java index 0e8feefa27..e1d16a0182 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java @@ -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]); diff --git a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java index 8baf401a4b..69430eb086 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java @@ -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 The number of inputs. * @return The normalizedInput */ - public static Matrix normalizeInputVector( + public static Matrix desaturateInputVector( Matrix u, double maxMagnitude) { double maxValue = u.maxAbs(); boolean isCapped = maxValue > maxMagnitude; diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java index b84eeba3ad..20a38a95cd 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java @@ -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. + * + *

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) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java index 3ea39e5d26..0807aba1c6 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java @@ -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) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java index 7a159fe661..ef354ef496 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java @@ -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. + * + *

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, 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 4c6d9cc33e..dc1b9e49f4 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 @@ -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. + * + *

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) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java index 8f3da6aab6..dcd4e58593 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java @@ -57,7 +57,7 @@ public class LinearSystemLoop(plant, dtSeconds), observer, - u -> StateSpaceUtil.normalizeInputVector(u, maxVoltageVolts)); + u -> StateSpaceUtil.desaturateInputVector(u, maxVoltageVolts)); } /** @@ -104,7 +104,7 @@ public class LinearSystemLoop StateSpaceUtil.normalizeInputVector(u, maxVoltageVolts)); + u -> StateSpaceUtil.desaturateInputVector(u, maxVoltageVolts)); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java index 37d8d68bbe..2164435954 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java @@ -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; diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java index b26cdcf517..dab3699650 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java @@ -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); diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java index 5d95290d23..5c2e1f58fe 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java @@ -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); diff --git a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp index 42d3018fa6..33c390d072 100644 --- a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp +++ b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp @@ -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)); diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp index fc47461a4f..dd30263fbc 100644 --- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp +++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp @@ -12,7 +12,7 @@ using namespace frc; -void MecanumDriveWheelSpeeds::Normalize( +void MecanumDriveWheelSpeeds::Desaturate( units::meters_per_second_t attainableMaxSpeed) { std::array wheelSpeeds{frontLeft, frontRight, rearLeft, rearRight}; diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp index d1c2f6f1fe..2a308dbc06 100644 --- a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp +++ b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp @@ -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; } diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp index 418a904b99..391b99f7ed 100644 --- a/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp +++ b/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp @@ -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); diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h index 730c4b90a8..5fdd18cafd 100644 --- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h +++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h @@ -336,8 +336,8 @@ Eigen::Vector 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 ClampInputMaxMagnitude( * @return The normalizedInput */ template -Eigen::Vector NormalizeInputVector( +Eigen::Vector DesaturateInputVector( const Eigen::Vector& u, double maxMagnitude) { double maxValue = u.template lpNorm(); diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h index 2bf9fb9738..fce2b9688a 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h @@ -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 diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h index c24b134542..e698c5f110 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h @@ -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 diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index 84057ac363..980109287a 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -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*, + * DesaturateWheelSpeeds(wpi::array*, * 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 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* moduleStates, units::meters_per_second_t attainableMaxSpeed); diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc index 1747453869..7b958c1ed1 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc @@ -88,7 +88,7 @@ ChassisSpeeds SwerveDriveKinematics::ToChassisSpeeds( } template -void SwerveDriveKinematics::NormalizeWheelSpeeds( +void SwerveDriveKinematics::DesaturateWheelSpeeds( wpi::array* moduleStates, units::meters_per_second_t attainableMaxSpeed) { auto& states = *moduleStates; diff --git a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h index 9ee2ea2ad4..a18c03bb19 100644 --- a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h +++ b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h @@ -54,7 +54,7 @@ class LinearSystemLoop { : LinearSystemLoop( plant, controller, observer, [=](const Eigen::Vector& u) { - return frc::NormalizeInputVector(u, maxVoltage.value()); + return frc::DesaturateInputVector(u, maxVoltage.value()); }, dt) {} @@ -99,7 +99,7 @@ class LinearSystemLoop { KalmanFilter& observer, units::volt_t maxVoltage) : LinearSystemLoop(controller, feedforward, observer, [=](const Eigen::Vector& u) { - return frc::NormalizeInputVector( + return frc::DesaturateInputVector( u, maxVoltage.value()); }) {} diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc b/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc index 1a1e4b869a..5c4e5ace9d 100644 --- a/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc +++ b/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc @@ -24,7 +24,7 @@ SwerveDriveKinematicsConstraint::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); diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java index d3346799c2..b3546f5cda 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java @@ -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; diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java index eec6aa2111..574e086272 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java @@ -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; diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp index 18b72de85e..8cc454e722 100644 --- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp @@ -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; diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp index 9384d891ee..55ebdcfd1f 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp @@ -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 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;