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] Replace MathUtil.clamp() with Java 21 Math.clamp() (#8186)

Browse Source
This commit is contained in:
ninjadrknss
2025-08-23 06:01:51 -10:00
committed by GitHub
parent 183328384b
commit c280fce147
26 changed files with 41 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java
View File

@@ -137,8 +137,8 @@ public class AnalogEncoder implements Sendable, AutoCloseable {
* @param max maximum voltage percentage (0-1 range)
*/
public void setVoltagePercentageRange(double min, double max) {
m_sensorMin = MathUtil.clamp(min, 0.0, 1.0);
m_sensorMax = MathUtil.clamp(max, 0.0, 1.0);
m_sensorMin = Math.clamp(min, 0.0, 1.0);
m_sensorMax = Math.clamp(max, 0.0, 1.0);
}
/**

4
wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java
View File

@@ -156,8 +156,8 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable {
* @param max maximum duty cycle (0-1 range)
*/
public void setDutyCycleRange(double min, double max) {
m_sensorMin = MathUtil.clamp(min, 0.0, 1.0);
m_sensorMax = MathUtil.clamp(max, 0.0, 1.0);
m_sensorMin = Math.clamp(min, 0.0, 1.0);
m_sensorMax = Math.clamp(max, 0.0, 1.0);
}
/**

3
wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java
View File

@@ -5,7 +5,6 @@
package edu.wpi.first.wpilibj;
import edu.wpi.first.hal.DriverStationJNI;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Pair;
import edu.wpi.first.wpilibj.DriverStation.POVDirection;
import edu.wpi.first.wpilibj.event.BooleanEvent;
@@ -467,7 +466,7 @@ public class GenericHID {
* @param value The normalized value (0 to 1) to set the rumble to
*/
public void setRumble(RumbleType type, double value) {
value = MathUtil.clamp(value, 0, 1);
value = Math.clamp(value, 0, 1);
int rumbleValue = (int) (value * 65535);
switch (type) {
case kLeftRumble -> this.m_leftRumble = rumbleValue;

9
wpilibj/src/main/java/edu/wpi/first/wpilibj/LEDPattern.java
View File

@@ -9,7 +9,6 @@ import static edu.wpi.first.units.Units.Microsecond;
import static edu.wpi.first.units.Units.Microseconds;
import static edu.wpi.first.units.Units.Value;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.units.collections.LongToObjectHashMap;
import edu.wpi.first.units.measure.Dimensionless;
import edu.wpi.first.units.measure.Distance;
@@ -479,9 +478,9 @@ public interface LEDPattern {
writer.setRGB(
i,
(int) MathUtil.clamp(r * multiplier, 0, 255),
(int) MathUtil.clamp(g * multiplier, 0, 255),
(int) MathUtil.clamp(b * multiplier, 0, 255));
(int) Math.clamp(r * multiplier, 0, 255),
(int) Math.clamp(g * multiplier, 0, 255),
(int) Math.clamp(b * multiplier, 0, 255));
});
};
}
@@ -526,7 +525,7 @@ public interface LEDPattern {
*/
static LEDPattern progressMaskLayer(DoubleSupplier progressSupplier) {
return (reader, writer) -> {
double progress = MathUtil.clamp(progressSupplier.getAsDouble(), 0, 1);
double progress = Math.clamp(progressSupplier.getAsDouble(), 0, 1);
int bufLen = reader.getLength();
int max = (int) (bufLen * progress);

3
wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java
View File

@@ -8,7 +8,6 @@ import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDevice.Direction;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.util.sendable.SendableRegistry;
@@ -81,7 +80,7 @@ public class Servo implements Sendable, AutoCloseable {
* @param value Position from 0.0 to 1.0.
*/
public void set(double value) {
value = MathUtil.clamp(value, 0.0, 1.0);
value = Math.clamp(value, 0.0, 1.0);
if (m_simPosition != null) {
m_simPosition.set(value);

5
wpilibj/src/main/java/edu/wpi/first/wpilibj/SharpIR.java
View File

@@ -8,7 +8,6 @@ import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDevice.Direction;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.util.sendable.SendableRegistry;
@@ -130,12 +129,12 @@ public class SharpIR implements Sendable, AutoCloseable {
*/
public double getRange() {
if (m_simRange != null) {
return MathUtil.clamp(m_simRange.get(), m_min, m_max);
return Math.clamp(m_simRange.get(), m_min, m_max);
} else {
// Don't allow zero/negative values
var v = Math.max(m_sensor.getVoltage(), 0.00001);
return MathUtil.clamp(m_A * Math.pow(v, m_B) * 1e-2, m_min, m_max);
return Math.clamp(m_A * Math.pow(v, m_B) * 1e-2, m_min, m_max);
}
}

12
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
View File

@@ -254,8 +254,8 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
* @return Wheel speeds [-1.0..1.0].
*/
public static WheelSpeeds arcadeDriveIK(double xSpeed, double zRotation, boolean squareInputs) {
xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
zRotation = MathUtil.clamp(zRotation, -1.0, 1.0);
xSpeed = Math.clamp(xSpeed, -1.0, 1.0);
zRotation = Math.clamp(zRotation, -1.0, 1.0);
// Square the inputs (while preserving the sign) to increase fine control
// while permitting full power.
@@ -295,8 +295,8 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
*/
public static WheelSpeeds curvatureDriveIK(
double xSpeed, double zRotation, boolean allowTurnInPlace) {
xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
zRotation = MathUtil.clamp(zRotation, -1.0, 1.0);
xSpeed = Math.clamp(xSpeed, -1.0, 1.0);
zRotation = Math.clamp(zRotation, -1.0, 1.0);
double leftSpeed;
double rightSpeed;
@@ -329,8 +329,8 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
* @return Wheel speeds [-1.0..1.0].
*/
public static WheelSpeeds tankDriveIK(double leftSpeed, double rightSpeed, boolean squareInputs) {
leftSpeed = MathUtil.clamp(leftSpeed, -1.0, 1.0);
rightSpeed = MathUtil.clamp(rightSpeed, -1.0, 1.0);
leftSpeed = Math.clamp(leftSpeed, -1.0, 1.0);
rightSpeed = Math.clamp(rightSpeed, -1.0, 1.0);
// Square the inputs (while preserving the sign) to increase fine control
// while permitting full power.

4
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java
View File

@@ -269,8 +269,8 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
*/
public static WheelSpeeds driveCartesianIK(
double xSpeed, double ySpeed, double zRotation, Rotation2d gyroAngle) {
xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0);
xSpeed = Math.clamp(xSpeed, -1.0, 1.0);
ySpeed = Math.clamp(ySpeed, -1.0, 1.0);
// Compensate for gyro angle.
var input = new Translation2d(xSpeed, ySpeed).rotateBy(gyroAngle.unaryMinus());

3
wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMMotorController.java
View File

@@ -7,7 +7,6 @@ package edu.wpi.first.wpilibj.motorcontrol;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDevice.Direction;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.util.sendable.SendableRegistry;
@@ -99,7 +98,7 @@ public abstract class PWMMotorController extends MotorSafety
*/
protected final void setSpeed(double speed) {
if (Double.isFinite(speed)) {
speed = MathUtil.clamp(speed, -1.0, 1.0);
speed = Math.clamp(speed, -1.0, 1.0);
} else {
speed = 0.0;
}

3
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
View File

@@ -4,7 +4,6 @@
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
@@ -135,7 +134,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
* @param velocity New velocity in meters per second.
*/
public final void setState(double position, double velocity) {
setState(VecBuilder.fill(MathUtil.clamp(position, m_minHeight, m_maxHeight), velocity));
setState(VecBuilder.fill(Math.clamp(position, m_minHeight, m_maxHeight), velocity));
}
/**

4
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java
View File

@@ -4,7 +4,6 @@
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
@@ -117,8 +116,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
* @param velocityRadPerSec The new angular velocity in radians per second.
*/
public final void setState(double angleRadians, double velocityRadPerSec) {
setState(
VecBuilder.fill(MathUtil.clamp(angleRadians, m_minAngle, m_maxAngle), velocityRadPerSec));
setState(VecBuilder.fill(Math.clamp(angleRadians, m_minAngle, m_maxAngle), velocityRadPerSec));
}
/**

2
wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java
View File

@@ -152,7 +152,7 @@ public class Color {
}
private static double roundAndClamp(double value) {
return MathUtil.clamp(Math.ceil(value * (1 << 12)) / (1 << 12), 0.0, 1.0);
return Math.clamp(Math.ceil(value * (1 << 12)) / (1 << 12), 0.0, 1.0);
}
// Helper methods

7
wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java
View File

@@ -4,7 +4,6 @@
package edu.wpi.first.wpilibj.util;
import edu.wpi.first.math.MathUtil;
import java.util.Objects;
/** Represents colors with 8 bits of precision. */
@@ -34,9 +33,9 @@ public class Color8Bit {
* @param blue Blue value (0-255)
*/
public Color8Bit(int red, int green, int blue) {
this.red = MathUtil.clamp(red, 0, 255);
this.green = MathUtil.clamp(green, 0, 255);
this.blue = MathUtil.clamp(blue, 0, 255);
this.red = Math.clamp(red, 0, 255);
this.green = Math.clamp(green, 0, 255);
this.blue = Math.clamp(blue, 0, 255);
}
/**

26
wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
View File

@@ -13,30 +13,6 @@ public final class MathUtil {
throw new AssertionError("utility class");
}
/**
* Returns value clamped between low and high boundaries.
*
* @param value Value to clamp.
* @param low The lower boundary to which to clamp value.
* @param high The higher boundary to which to clamp value.
* @return The clamped value.
*/
public static int clamp(int value, int low, int high) {
return Math.max(low, Math.min(value, high));
}
/**
* Returns value clamped between low and high boundaries.
*
* @param value Value to clamp.
* @param low The lower boundary to which to clamp value.
* @param high The higher boundary to which to clamp value.
* @return The clamped value.
*/
public static double clamp(double value, double low, double high) {
return Math.max(low, Math.min(value, high));
}
/**
* Returns 0.0 if the given value is within the specified range around zero. The remaining range
* between the deadband and the maximum magnitude is scaled from 0.0 to the maximum magnitude.
@@ -184,7 +160,7 @@ public final class MathUtil {
* @return The interpolated value.
*/
public static double interpolate(double startValue, double endValue, double t) {
return startValue + (endValue - startValue) * MathUtil.clamp(t, 0, 1);
return startValue + (endValue - startValue) * Math.clamp(t, 0, 1);
}
/**

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

@@ -152,7 +152,7 @@ public final class StateSpaceUtil {
Matrix<I, N1> u, Matrix<I, N1> umin, Matrix<I, N1> umax) {
var result = new Matrix<I, N1>(new SimpleMatrix(u.getNumRows(), 1));
for (int i = 0; i < u.getNumRows(); i++) {
result.set(i, 0, MathUtil.clamp(u.get(i, 0), umin.get(i, 0), umax.get(i, 0)));
result.set(i, 0, Math.clamp(u.get(i, 0), umin.get(i, 0), umax.get(i, 0)));
}
return result;
}

2
wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
View File

@@ -455,7 +455,7 @@ public class PIDController implements Sendable, AutoCloseable {
m_totalError = 0;
} else if (m_ki != 0) {
m_totalError =
MathUtil.clamp(
Math.clamp(
m_totalError + m_error * m_period,
m_minimumIntegral / m_ki,
m_maximumIntegral / m_ki);

3
wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
View File

@@ -5,7 +5,6 @@
package edu.wpi.first.math.estimator;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
@@ -187,7 +186,7 @@ public class PoseEstimator<T> {
// the buffer will always use a timestamp between the first and last timestamps)
double oldestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().firstKey();
double newestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().lastKey();
timestamp = MathUtil.clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp);
timestamp = Math.clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp);
// Step 2: If there are no applicable vision updates, use the odometry-only information.
if (m_visionUpdates.isEmpty() || timestamp < m_visionUpdates.firstKey()) {

3
wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java
View File

@@ -5,7 +5,6 @@
package edu.wpi.first.math.estimator;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
@@ -199,7 +198,7 @@ public class PoseEstimator3d<T> {
// the buffer will always use a timestamp between the first and last timestamps)
double oldestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().firstKey();
double newestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().lastKey();
timestamp = MathUtil.clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp);
timestamp = Math.clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp);
// Step 2: If there are no applicable vision updates, use the odometry-only information.
if (m_visionUpdates.isEmpty() || timestamp < m_visionUpdates.firstKey()) {

3
wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java
View File

@@ -5,7 +5,6 @@
package edu.wpi.first.math.filter;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUtil;
/**
* A class that limits the rate of change of an input value. Useful for implementing voltage,
@@ -56,7 +55,7 @@ public class SlewRateLimiter {
double currentTime = MathSharedStore.getTimestamp();
double elapsedTime = currentTime - m_prevTime;
m_prevVal +=
MathUtil.clamp(
Math.clamp(
input - m_prevVal,
m_negativeRateLimit * elapsedTime,
m_positiveRateLimit * elapsedTime);

5
wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java
View File

@@ -6,7 +6,6 @@ package edu.wpi.first.math.geometry;
import static edu.wpi.first.units.Units.Meters;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.proto.Rectangle2dProto;
import edu.wpi.first.math.geometry.struct.Rectangle2dStruct;
import edu.wpi.first.units.measure.Distance;
@@ -217,9 +216,9 @@ public class Rectangle2d implements ProtobufSerializable, StructSerializable {
// Find nearest point
point =
new Translation2d(
MathUtil.clamp(
Math.clamp(
point.getX(), m_center.getX() - m_xWidth / 2.0, m_center.getX() + m_xWidth / 2.0),
MathUtil.clamp(
Math.clamp(
point.getY(), m_center.getY() - m_yWidth / 2.0, m_center.getY() + m_yWidth / 2.0));
// Undo rotation

3
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
View File

@@ -12,7 +12,6 @@ import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.geometry.proto.Rotation2dProto;
@@ -364,7 +363,7 @@ public class Rotation2d
@Override
public Rotation2d interpolate(Rotation2d endValue, double t) {
return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
return plus(endValue.minus(this).times(Math.clamp(t, 0, 1)));
}
/** Rotation2d protobuf for serialization. */

3
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
View File

@@ -12,7 +12,6 @@ import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
@@ -562,7 +561,7 @@ public class Rotation3d
@Override
public Rotation3d interpolate(Rotation3d endValue, double t) {
return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
return plus(endValue.minus(this).times(Math.clamp(t, 0, 1)));
}
/** Rotation3d protobuf for serialization. */

15
wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
View File

@@ -18,21 +18,6 @@ class MathUtilTest extends UtilityClassTest<MathUtil> {
super(MathUtil.class);
}
@Test
void testClamp() {
// int
assertEquals(5, MathUtil.clamp(10, 1, 5));
// double
assertEquals(5.5, MathUtil.clamp(10.5, 1.5, 5.5));
// negative int
assertEquals(-5, MathUtil.clamp(-10, -5, -1));
// negative double
assertEquals(-5.5, MathUtil.clamp(-10.5, -5.5, -1.5));
}
@Test
void testApplyDeadbandUnityScale() {
// < 0

3
wpimath/src/test/java/edu/wpi/first/math/estimator/MerweUKFTest.java
View File

@@ -9,7 +9,6 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.StateSpaceUtil;
@@ -297,7 +296,7 @@ class MerweUKFTest {
return MatBuilder.fill(
Nat.N1(),
Nat.N1(),
MathUtil.clamp(
Math.clamp(
8 * Math.sin(Math.PI * Math.sqrt(2.0) * t)
+ 6 * Math.sin(Math.PI * Math.sqrt(3.0) * t)
+ 4 * Math.sin(Math.PI * Math.sqrt(5.0) * t),

3
wpimath/src/test/java/edu/wpi/first/math/estimator/S3UKFTest.java
View File

@@ -9,7 +9,6 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.StateSpaceUtil;
@@ -297,7 +296,7 @@ class S3UKFTest {
return MatBuilder.fill(
Nat.N1(),
Nat.N1(),
MathUtil.clamp(
Math.clamp(
8 * Math.sin(Math.PI * Math.sqrt(2.0) * t)
+ 6 * Math.sin(Math.PI * Math.sqrt(3.0) * t)
+ 4 * Math.sin(Math.PI * Math.sqrt(5.0) * t),

5
wpimath/src/test/java/edu/wpi/first/math/optimization/SimulatedAnnealingTest.java
View File

@@ -6,7 +6,6 @@ package edu.wpi.first.math.optimization;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.MathUtil;
import java.util.function.DoubleUnaryOperator;
import org.junit.jupiter.api.Test;
@@ -20,7 +19,7 @@ class SimulatedAnnealingTest {
var simulatedAnnealing =
new SimulatedAnnealing<Double>(
2.0,
x -> MathUtil.clamp(x + (Math.random() - 0.5) * stepSize, -3, 3),
x -> Math.clamp(x + (Math.random() - 0.5) * stepSize, -3, 3),
function::applyAsDouble);
double solution = simulatedAnnealing.solve(-1.0, 5000);
@@ -37,7 +36,7 @@ class SimulatedAnnealingTest {
var simulatedAnnealing =
new SimulatedAnnealing<Double>(
2.0,
x -> MathUtil.clamp(x + (Math.random() - 0.5) * stepSize, 0, 7),
x -> Math.clamp(x + (Math.random() - 0.5) * stepSize, 0, 7),
function::applyAsDouble);
double solution = simulatedAnnealing.solve(-1.0, 5000);