diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java index cdde0ffb7c..f4fb80ed81 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java @@ -110,8 +110,7 @@ public class Elevator implements AutoCloseable { // With the setpoint value we run PID control like normal double pidOutput = m_pidController.calculate(m_encoder.getDistance(), m_setpoint.position); - double feedforwardOutput = - m_feedforward.calculateWithVelocities(m_setpoint.velocity, next.velocity); + double feedforwardOutput = m_feedforward.calculate(m_setpoint.velocity, next.velocity); m_motor.setVoltage(pidOutput + feedforwardOutput); diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java index d918f07fd8..fb26ae1331 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java @@ -131,37 +131,18 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable return m_dt; } - /** - * Calculates the feedforward from the gains and setpoints. - * - * @param positionRadians The position (angle) setpoint. This angle should be measured from the - * horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If - * your encoder does not follow this convention, an offset should be added. - * @param velocityRadPerSec The velocity setpoint. - * @param accelRadPerSecSquared The acceleration setpoint. - * @return The computed feedforward. - */ - @Deprecated(forRemoval = true, since = "2025") - public double calculate( - double positionRadians, double velocityRadPerSec, double accelRadPerSecSquared) { - return ks * Math.signum(velocityRadPerSec) - + kg * Math.cos(positionRadians) - + kv * velocityRadPerSec - + ka * accelRadPerSecSquared; - } - /** * Calculates the feedforward from the gains and velocity setpoint assuming continuous control * (acceleration is assumed to be zero). * - * @param positionRadians The position (angle) setpoint. This angle should be measured from the + * @param position The position setpoint in radians. This angle should be measured from the * horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If * your encoder does not follow this convention, an offset should be added. - * @param velocity The velocity setpoint. + * @param velocity The velocity setpoint in radians per second. * @return The computed feedforward. */ - public double calculate(double positionRadians, double velocity) { - return calculate(positionRadians, velocity, 0); + public double calculate(double position, double velocity) { + return ks * Math.signum(velocity) + kg * Math.cos(position) + kv * velocity; } /** @@ -172,29 +153,9 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable * your encoder does not follow this convention, an offset should be added. * @param currentVelocity The current velocity setpoint in radians per second. * @param nextVelocity The next velocity setpoint in radians per second. - * @param dt Time between velocity setpoints in seconds. * @return The computed feedforward in volts. */ - @SuppressWarnings("removal") - @Deprecated(forRemoval = true, since = "2025") - public double calculate( - double currentAngle, double currentVelocity, double nextVelocity, double dt) { - return ArmFeedforwardJNI.calculate( - ks, kv, ka, kg, currentAngle, currentVelocity, nextVelocity, dt); - } - - /** - * Calculates the feedforward from the gains and setpoints assuming discrete control. - * - * @param currentAngle The current angle in radians. This angle should be measured from the - * horizontal (i.e. if the provided angle is 0, the arm should be parallel to the floor). If - * your encoder does not follow this convention, an offset should be added. - * @param currentVelocity The current velocity setpoint in radians per second. - * @param nextVelocity The next velocity setpoint in radians per second. - * @return The computed feedforward in volts. - */ - public double calculateWithVelocities( - double currentAngle, double currentVelocity, double nextVelocity) { + public double calculate(double currentAngle, double currentVelocity, double nextVelocity) { return ArmFeedforwardJNI.calculate( ks, kv, ka, kg, currentAngle, currentVelocity, nextVelocity, m_dt); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java index 60060bb610..71601612ea 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java @@ -131,28 +131,15 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ return m_dt; } - /** - * Calculates the feedforward from the gains and setpoints assuming continuous control. - * - * @param velocity The velocity setpoint. - * @param acceleration The acceleration setpoint. - * @return The computed feedforward. - */ - @SuppressWarnings("removal") - @Deprecated(forRemoval = true, since = "2025") - public double calculate(double velocity, double acceleration) { - return ks * Math.signum(velocity) + kg + kv * velocity + ka * acceleration; - } - /** * Calculates the feedforward from the gains and velocity setpoint assuming continuous control * (acceleration is assumed to be zero). * - * @param velocity The velocity setpoint. + * @param velocity The velocity setpoint in meters per second. * @return The computed feedforward. */ public double calculate(double velocity) { - return calculate(velocity, 0); + return calculate(velocity, velocity); } /** @@ -162,9 +149,9 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ * * @param currentVelocity The current velocity setpoint in meters per second. * @param nextVelocity The next velocity setpoint in meters per second. - * @return The computed feedforward in volts. + * @return The computed feedforward. */ - public double calculateWithVelocities(double currentVelocity, double nextVelocity) { + public double calculate(double currentVelocity, double nextVelocity) { // See wpimath/algorithms.md#Elevator_feedforward for derivation if (ka == 0.0) { return ks * Math.signum(nextVelocity) + kg + kv * nextVelocity; diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java index 3c8748d35f..eb336089e4 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java @@ -68,8 +68,7 @@ class ArmFeedforwardTest { */ private void calculateAndSimulate( double currentAngle, double currentVelocity, double nextVelocity, double dt) { - final double input = - m_armFF.calculateWithVelocities(currentAngle, currentVelocity, nextVelocity); + final double input = m_armFF.calculate(currentAngle, currentVelocity, nextVelocity); assertEquals(nextVelocity, simulate(currentAngle, currentVelocity, input, dt).get(1, 0), 1e-12); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java index 92dd9cb396..2f28e01635 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java @@ -36,7 +36,7 @@ class ElevatorFeedforwardTest { var nextR = VecBuilder.fill(3.0); assertEquals( plantInversion.calculate(r, nextR).get(0, 0) + ks + kg, - m_elevatorFF.calculateWithVelocities(2.0, 3.0), + m_elevatorFF.calculate(2.0, 3.0), 0.002); }