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 3b2765a990..cdde0ffb7c 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 @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.elevatorexponentialsimulation.subsystems; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; @@ -114,9 +111,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 - .calculate(MetersPerSecond.of(m_setpoint.velocity), MetersPerSecond.of(next.velocity)) - .in(Volts); + m_feedforward.calculateWithVelocities(m_setpoint.velocity, next.velocity); m_motor.setVoltage(pidOutput + feedforwardOutput); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java index 1938c70fa4..ee37915d67 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.elevatorprofiledpid; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -54,8 +51,6 @@ public class Robot extends TimedRobot { // Run controller and update motor output m_motor.setVoltage( m_controller.calculate(m_encoder.getDistance()) - + m_feedforward - .calculate(MetersPerSecond.of(m_controller.getSetpoint().velocity)) - .in(Volts)); + + m_feedforward.calculate(m_controller.getSetpoint().velocity)); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java index 90378bd119..d4f04fed26 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.elevatorsimulation.subsystems; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.system.plant.DCMotor; @@ -104,8 +101,7 @@ public class Elevator implements AutoCloseable { // With the setpoint value we run PID control like normal double pidOutput = m_controller.calculate(m_encoder.getDistance()); - double feedforwardOutput = - m_feedforward.calculate(MetersPerSecond.of(m_controller.getSetpoint().velocity)).in(Volts); + double feedforwardOutput = m_feedforward.calculate(m_controller.getSetpoint().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 2f9ffe6ce7..d918f07fd8 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 @@ -4,16 +4,9 @@ package edu.wpi.first.math.controller; -import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.proto.ArmFeedforwardProto; import edu.wpi.first.math.controller.struct.ArmFeedforwardStruct; import edu.wpi.first.math.jni.ArmFeedforwardJNI; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; @@ -167,8 +160,6 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable * @param velocity The velocity setpoint. * @return The computed feedforward. */ - @SuppressWarnings("removal") - @Deprecated(forRemoval = true, since = "2025") public double calculate(double positionRadians, double velocity) { return calculate(positionRadians, velocity, 0); } @@ -192,45 +183,20 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable ks, kv, ka, kg, currentAngle, currentVelocity, nextVelocity, dt); } - /** - * Calculates the feedforward from the gains and setpoints assuming discrete control when the - * velocity does not change. - * - * @param currentAngle The current angle. 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. - * @return The computed feedforward in volts. - */ - public Voltage calculate(Angle currentAngle, AngularVelocity currentVelocity) { - return Volts.of( - kg * Math.cos(currentAngle.in(Radians)) - + ks * Math.signum(currentVelocity.in(RadiansPerSecond)) - + kv * currentVelocity.in(RadiansPerSecond)); - } - /** * Calculates the feedforward from the gains and setpoints assuming discrete control. * - * @param currentAngle The current angle. 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. - * @param nextVelocity The next velocity setpoint. + * @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 Voltage calculate( - Angle currentAngle, AngularVelocity currentVelocity, AngularVelocity nextVelocity) { - return Volts.of( - ArmFeedforwardJNI.calculate( - ks, - kv, - ka, - kg, - currentAngle.in(Radians), - currentVelocity.in(RadiansPerSecond), - nextVelocity.in(RadiansPerSecond), - m_dt)); + public double calculateWithVelocities( + double currentAngle, double currentVelocity, double nextVelocity) { + return ArmFeedforwardJNI.calculate( + ks, kv, ka, kg, currentAngle, currentVelocity, nextVelocity, m_dt); } // Rearranging the main equation from the calculate() method yields the 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 4421270088..60060bb610 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 @@ -4,13 +4,8 @@ package edu.wpi.first.math.controller; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.proto.ElevatorFeedforwardProto; import edu.wpi.first.math.controller.struct.ElevatorFeedforwardStruct; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; @@ -156,50 +151,31 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ * @param velocity The velocity setpoint. * @return The computed feedforward. */ - @SuppressWarnings("removal") - @Deprecated(forRemoval = true, since = "2025") public double calculate(double velocity) { return calculate(velocity, 0); } - /** - * Calculates the feedforward from the gains and setpoints assuming discrete control when the - * setpoint does not change. - * - * @param currentVelocity The velocity setpoint. - * @return The computed feedforward. - */ - public Voltage calculate(LinearVelocity currentVelocity) { - return calculate(currentVelocity, currentVelocity); - } - /** * Calculates the feedforward from the gains and setpoints assuming discrete control. * *

Note this method is inaccurate when the velocity crosses 0. * - * @param currentVelocity The current velocity setpoint. - * @param nextVelocity The next velocity setpoint. - * @return The computed feedforward. + * @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. */ - public Voltage calculate(LinearVelocity currentVelocity, LinearVelocity nextVelocity) { + public double calculateWithVelocities(double currentVelocity, double nextVelocity) { // See wpimath/algorithms.md#Elevator_feedforward for derivation if (ka == 0.0) { - return Volts.of( - ks * Math.signum(nextVelocity.in(MetersPerSecond)) - + kg - + kv * nextVelocity.in(MetersPerSecond)); + return ks * Math.signum(nextVelocity) + kg + kv * nextVelocity; } else { double A = -kv / ka; double B = 1.0 / ka; double A_d = Math.exp(A * m_dt); double B_d = 1.0 / A * (A_d - 1.0) * B; - return Volts.of( - kg - + ks * Math.signum(currentVelocity.magnitude()) - + 1.0 - / B_d - * (nextVelocity.in(MetersPerSecond) - A_d * currentVelocity.in(MetersPerSecond))); + return kg + + ks * Math.signum(currentVelocity) + + 1.0 / B_d * (nextVelocity - A_d * currentVelocity); } } 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 ecfddcf6be..3c8748d35f 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 @@ -4,9 +4,6 @@ package edu.wpi.first.math.controller; -import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Volts; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertThrows; @@ -72,22 +69,15 @@ class ArmFeedforwardTest { private void calculateAndSimulate( double currentAngle, double currentVelocity, double nextVelocity, double dt) { final double input = - m_armFF - .calculate( - Radians.of(currentAngle), - RadiansPerSecond.of(currentVelocity), - RadiansPerSecond.of(nextVelocity)) - .in(Volts); + m_armFF.calculateWithVelocities(currentAngle, currentVelocity, nextVelocity); assertEquals(nextVelocity, simulate(currentAngle, currentVelocity, input, dt).get(1, 0), 1e-12); } @Test void testCalculate() { // calculate(angle, angular velocity) - assertEquals( - 0.5, m_armFF.calculate(Radians.of(Math.PI / 3), RadiansPerSecond.of(0)).in(Volts), 0.002); - assertEquals( - 2.5, m_armFF.calculate(Radians.of(Math.PI / 3), RadiansPerSecond.of(1)).in(Volts), 0.002); + assertEquals(0.5, m_armFF.calculate(Math.PI / 3, 0.0), 0.002); + assertEquals(2.5, m_armFF.calculate(Math.PI / 3, 1.0), 0.002); // calculate(currentAngle, currentVelocity, nextAngle, dt) calculateAndSimulate(Math.PI / 3, 1.0, 1.05, 0.020); 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 c4161a0b97..92dd9cb396 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 @@ -4,8 +4,6 @@ package edu.wpi.first.math.controller; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertThrows; @@ -26,8 +24,8 @@ class ElevatorFeedforwardTest { @Test void testCalculate() { - assertEquals(1, m_elevatorFF.calculate(MetersPerSecond.of(0)).in(Volts), 0.002); - assertEquals(4.5, m_elevatorFF.calculate(MetersPerSecond.of(2)).in(Volts), 0.002); + assertEquals(1, m_elevatorFF.calculate(0.0), 0.002); + assertEquals(4.5, m_elevatorFF.calculate(2.0), 0.002); var A = MatBuilder.fill(Nat.N1(), Nat.N1(), -kv / ka); var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / ka); @@ -38,7 +36,7 @@ class ElevatorFeedforwardTest { var nextR = VecBuilder.fill(3.0); assertEquals( plantInversion.calculate(r, nextR).get(0, 0) + ks + kg, - m_elevatorFF.calculate(MetersPerSecond.of(2.0), MetersPerSecond.of(3.0)).in(Volts), + m_elevatorFF.calculateWithVelocities(2.0, 3.0), 0.002); }