From caebbedfa310053776f589141968f4316d3ce0aa Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Thu, 5 Dec 2024 00:12:03 -0800 Subject: [PATCH] [wpimath] Improve SimpleMotorFeedforward argument names and deprecation message Also roll back SimpleMotorFeedforward unit API until 2027. --- .../command/MecanumControllerCommand.java | 64 ++++++++----------- .../wpilibj2/command/RamseteCommand.java | 24 +++---- .../differentialdrivebot/Drivetrain.java | 9 +-- .../Drivetrain.java | 9 +-- .../subsystems/DriveSubsystem.java | 13 +--- .../elevatorexponentialprofile/Robot.java | 5 +- .../elevatortrapezoidprofile/Robot.java | 5 +- .../wpilibj/examples/eventloop/Robot.java | 5 +- .../flywheelbangbangcontroller/Robot.java | 6 +- .../examples/mecanumbot/Drivetrain.java | 15 ++--- .../mecanumdriveposeestimator/Drivetrain.java | 15 ++--- .../subsystems/Drive.java | 7 +- .../subsystems/Shooter.java | 6 +- .../Drivetrain.java | 9 +-- .../examples/swervebot/SwerveModule.java | 13 +--- .../SwerveModule.java | 13 +--- .../sysidroutine/subsystems/Shooter.java | 4 +- .../controller/SimpleMotorFeedforward.java | 35 ++-------- .../frc/controller/SimpleMotorFeedforward.h | 11 ++-- .../SimpleMotorFeedforwardTest.java | 11 ++-- ...ifferentialDriveVoltageConstraintTest.java | 38 ++++------- .../trajectory/ExponentialProfileTest.java | 7 +- 22 files changed, 91 insertions(+), 233 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 bb47e58a370..70978c63f12 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 @@ -4,8 +4,6 @@ package edu.wpi.first.wpilibj2.command; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; import edu.wpi.first.math.controller.HolonomicDriveController; @@ -19,7 +17,6 @@ import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages; import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds; import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.units.measure.MutLinearVelocity; import edu.wpi.first.wpilibj.Timer; import java.util.function.Consumer; import java.util.function.Supplier; @@ -58,14 +55,10 @@ public class MecanumControllerCommand extends Command { private final Supplier m_currentWheelSpeeds; private final Consumer m_outputDriveVoltages; private final Consumer m_outputWheelSpeeds; - private final MutLinearVelocity m_prevFrontLeftSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_prevRearLeftSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_prevFrontRightSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_prevRearRightSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_frontLeftSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_rearLeftSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_frontRightSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_rearRightSpeedSetpoint = MetersPerSecond.mutable(0); + private double m_prevFrontLeftSpeedSetpoint; // m/s + private double m_prevRearLeftSpeedSetpoint; // m/s + private double m_prevFrontRightSpeedSetpoint; // m/s + private double m_prevRearRightSpeedSetpoint; // m/s /** * Constructs a new MecanumControllerCommand that when executed will follow the provided @@ -343,10 +336,10 @@ public void initialize() { MecanumDriveWheelSpeeds prevSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0)); - m_prevFrontLeftSpeedSetpoint.mut_setMagnitude(prevSpeeds.frontLeftMetersPerSecond); - m_prevRearLeftSpeedSetpoint.mut_setMagnitude(prevSpeeds.rearLeftMetersPerSecond); - m_prevFrontRightSpeedSetpoint.mut_setMagnitude(prevSpeeds.frontRightMetersPerSecond); - m_prevRearRightSpeedSetpoint.mut_setMagnitude(prevSpeeds.rearRightMetersPerSecond); + m_prevFrontLeftSpeedSetpoint = prevSpeeds.frontLeftMetersPerSecond; + m_prevRearLeftSpeedSetpoint = prevSpeeds.rearLeftMetersPerSecond; + m_prevFrontRightSpeedSetpoint = prevSpeeds.frontRightMetersPerSecond; + m_prevRearRightSpeedSetpoint = prevSpeeds.rearRightMetersPerSecond; m_timer.restart(); } @@ -363,10 +356,10 @@ public void execute() { targetWheelSpeeds.desaturate(m_maxWheelVelocityMetersPerSecond); - m_frontLeftSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.frontLeftMetersPerSecond); - m_rearLeftSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.rearLeftMetersPerSecond); - m_frontRightSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.frontRightMetersPerSecond); - m_rearRightSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.rearRightMetersPerSecond); + double frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond; + double rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond; + double frontRightSpeedSetpoint = targetWheelSpeeds.frontRightMetersPerSecond; + double rearRightSpeedSetpoint = targetWheelSpeeds.rearRightMetersPerSecond; double frontLeftOutput; double rearLeftOutput; @@ -375,42 +368,39 @@ public void execute() { if (m_usePID) { final double frontLeftFeedforward = - m_feedforward.calculate(m_prevFrontLeftSpeedSetpoint, m_frontLeftSpeedSetpoint).in(Volts); + m_feedforward.calculateWithVelocities( + m_prevFrontLeftSpeedSetpoint, frontLeftSpeedSetpoint); final double rearLeftFeedforward = - m_feedforward.calculate(m_prevRearLeftSpeedSetpoint, m_rearLeftSpeedSetpoint).in(Volts); + m_feedforward.calculateWithVelocities(m_prevRearLeftSpeedSetpoint, rearLeftSpeedSetpoint); final double frontRightFeedforward = - m_feedforward - .calculate(m_prevFrontRightSpeedSetpoint, m_frontRightSpeedSetpoint) - .in(Volts); + m_feedforward.calculateWithVelocities( + m_prevFrontRightSpeedSetpoint, frontRightSpeedSetpoint); final double rearRightFeedforward = - m_feedforward.calculate(m_prevRearRightSpeedSetpoint, m_rearRightSpeedSetpoint).in(Volts); + m_feedforward.calculateWithVelocities( + m_prevRearRightSpeedSetpoint, rearRightSpeedSetpoint); frontLeftOutput = frontLeftFeedforward + m_frontLeftController.calculate( - m_currentWheelSpeeds.get().frontLeftMetersPerSecond, - m_frontLeftSpeedSetpoint.in(MetersPerSecond)); + m_currentWheelSpeeds.get().frontLeftMetersPerSecond, frontLeftSpeedSetpoint); rearLeftOutput = rearLeftFeedforward + m_rearLeftController.calculate( - m_currentWheelSpeeds.get().rearLeftMetersPerSecond, - m_rearLeftSpeedSetpoint.in(MetersPerSecond)); + m_currentWheelSpeeds.get().rearLeftMetersPerSecond, rearLeftSpeedSetpoint); frontRightOutput = frontRightFeedforward + m_frontRightController.calculate( - m_currentWheelSpeeds.get().frontRightMetersPerSecond, - m_frontRightSpeedSetpoint.in(MetersPerSecond)); + m_currentWheelSpeeds.get().frontRightMetersPerSecond, frontRightSpeedSetpoint); rearRightOutput = rearRightFeedforward + m_rearRightController.calculate( - m_currentWheelSpeeds.get().rearRightMetersPerSecond, - m_rearRightSpeedSetpoint.in(MetersPerSecond)); + m_currentWheelSpeeds.get().rearRightMetersPerSecond, rearRightSpeedSetpoint); m_outputDriveVoltages.accept( new MecanumDriveMotorVoltages( @@ -419,10 +409,10 @@ public void execute() { } else { m_outputWheelSpeeds.accept( new MecanumDriveWheelSpeeds( - m_frontLeftSpeedSetpoint.in(MetersPerSecond), - m_frontRightSpeedSetpoint.in(MetersPerSecond), - m_rearLeftSpeedSetpoint.in(MetersPerSecond), - m_rearRightSpeedSetpoint.in(MetersPerSecond))); + frontLeftSpeedSetpoint, + frontRightSpeedSetpoint, + rearLeftSpeedSetpoint, + rearRightSpeedSetpoint)); } } diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java index 533db5b0398..88aae9cca16 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java @@ -4,8 +4,6 @@ package edu.wpi.first.wpilibj2.command; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; import edu.wpi.first.math.controller.PIDController; @@ -16,7 +14,6 @@ import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.units.measure.MutLinearVelocity; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.Timer; import java.util.function.BiConsumer; @@ -49,10 +46,8 @@ public class RamseteCommand extends Command { private final PIDController m_rightController; private final BiConsumer m_output; private DifferentialDriveWheelSpeeds m_prevSpeeds = new DifferentialDriveWheelSpeeds(); - private final MutLinearVelocity m_prevLeftSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_prevRightSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_leftSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_rightSpeedSetpoint = MetersPerSecond.mutable(0); + private double m_prevLeftSpeedSetpoint; // m/s + private double m_prevRightSpeedSetpoint; // m/s private double m_prevTime; /** @@ -156,8 +151,8 @@ public void initialize() { initialState.velocityMetersPerSecond, 0, initialState.curvatureRadPerMeter * initialState.velocityMetersPerSecond)); - m_prevLeftSpeedSetpoint.mut_setMagnitude(m_prevSpeeds.leftMetersPerSecond); - m_prevRightSpeedSetpoint.mut_setMagnitude(m_prevSpeeds.rightMetersPerSecond); + m_prevLeftSpeedSetpoint = m_prevSpeeds.leftMetersPerSecond; + m_prevRightSpeedSetpoint = m_prevSpeeds.rightMetersPerSecond; m_timer.restart(); if (m_usePID) { m_leftController.reset(); @@ -179,21 +174,18 @@ public void execute() { m_kinematics.toWheelSpeeds( m_follower.calculate(m_pose.get(), m_trajectory.sample(curTime))); - var leftSpeedSetpoint = targetWheelSpeeds.leftMetersPerSecond; - var rightSpeedSetpoint = targetWheelSpeeds.rightMetersPerSecond; - - m_leftSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.leftMetersPerSecond); - m_rightSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.rightMetersPerSecond); + double leftSpeedSetpoint = targetWheelSpeeds.leftMetersPerSecond; + double rightSpeedSetpoint = targetWheelSpeeds.rightMetersPerSecond; double leftOutput; double rightOutput; if (m_usePID) { double leftFeedforward = - m_feedforward.calculate(m_prevLeftSpeedSetpoint, m_leftSpeedSetpoint).in(Volts); + m_feedforward.calculateWithVelocities(m_prevLeftSpeedSetpoint, leftSpeedSetpoint); double rightFeedforward = - m_feedforward.calculate(m_prevRightSpeedSetpoint, m_rightSpeedSetpoint).in(Volts); + m_feedforward.calculateWithVelocities(m_prevRightSpeedSetpoint, rightSpeedSetpoint); leftOutput = leftFeedforward diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java index b351d169db4..adc52e2ef67 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.differentialdrivebot; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -82,10 +79,8 @@ public Drivetrain() { * @param speeds The desired wheel speeds. */ public void setSpeeds(DifferentialDriveWheelSpeeds speeds) { - final double leftFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.leftMetersPerSecond)).in(Volts); - final double rightFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.rightMetersPerSecond)).in(Volts); + final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond); + final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond); final double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java index 7df9f3b4496..fbf6e90b9c9 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.ComputerVisionUtil; @@ -142,10 +139,8 @@ public Drivetrain(DoubleArrayTopic cameraToObjectTopic) { * @param speeds The desired wheel speeds. */ public void setSpeeds(DifferentialDriveWheelSpeeds speeds) { - final double leftFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.leftMetersPerSecond)).in(Volts); - final double rightFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.rightMetersPerSecond)).in(Volts); + final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond); + final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond); final double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java index 67fea4c8ae0..b8947ce6da9 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; @@ -99,18 +96,12 @@ public void setDriveStates( m_leftLeader.setSetpoint( ExampleSmartMotorController.PIDMode.kPosition, currentLeft.position, - m_feedforward - .calculate( - MetersPerSecond.of(currentLeft.velocity), MetersPerSecond.of(nextLeft.velocity)) - .in(Volts) + m_feedforward.calculateWithVelocities(currentLeft.velocity, nextLeft.velocity) / RobotController.getBatteryVoltage()); m_rightLeader.setSetpoint( ExampleSmartMotorController.PIDMode.kPosition, currentRight.position, - m_feedforward - .calculate( - MetersPerSecond.of(currentLeft.velocity), MetersPerSecond.of(nextLeft.velocity)) - .in(Volts) + m_feedforward.calculateWithVelocities(currentLeft.velocity, nextLeft.velocity) / RobotController.getBatteryVoltage()); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java index 83958230ec6..373485923c5 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.elevatorexponentialprofile; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.trajectory.ExponentialProfile; import edu.wpi.first.wpilibj.Joystick; @@ -48,7 +45,7 @@ public void teleopPeriodic() { m_motor.setSetpoint( ExampleSmartMotorController.PIDMode.kPosition, m_setpoint.position, - m_feedforward.calculate(RadiansPerSecond.of(next.velocity)).in(Volts) / 12.0); + m_feedforward.calculate(next.velocity) / 12.0); m_setpoint = next; } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java index e1ae800421d..2ab9560dcb8 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.Joystick; @@ -48,6 +45,6 @@ public void teleopPeriodic() { m_motor.setSetpoint( ExampleSmartMotorController.PIDMode.kPosition, m_setpoint.position, - m_feedforward.calculate(MetersPerSecond.of(m_setpoint.velocity)).in(Volts) / 12.0); + m_feedforward.calculate(m_setpoint.velocity) / 12.0); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java index e75edfe3d19..f9646e282a3 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.eventloop; -import static edu.wpi.first.units.Units.RPM; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj.Encoder; @@ -67,7 +64,7 @@ public Robot() { () -> { m_shooter.setVoltage( m_controller.calculate(m_shooterEncoder.getRate(), SHOT_VELOCITY) - + m_ff.calculate(RPM.of(SHOT_VELOCITY)).in(Volts)); + + m_ff.calculate(SHOT_VELOCITY)); }); // if not, stop diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java index abba8a7365c..76fc4317594 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.flywheelbangbangcontroller; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.BangBangController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.numbers.N1; @@ -89,8 +86,7 @@ public void teleopPeriodic() { // Controls a motor with the output of the BangBang controller and a // feedforward. The feedforward is reduced slightly to avoid overspeeding // the shooter. - m_flywheelMotor.setVoltage( - bangOutput + 0.9 * m_feedforward.calculate(RadiansPerSecond.of(setpoint)).in(Volts)); + m_flywheelMotor.setVoltage(bangOutput + 0.9 * m_feedforward.calculate(setpoint)); } /** Update our simulation. This should be run every robot loop in simulation. */ 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 addedc0a9a7..61cfce638ec 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 @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.mecanumbot; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Translation2d; @@ -98,14 +95,10 @@ public MecanumDriveWheelPositions getCurrentDistances() { * @param speeds The desired wheel speeds. */ public void setSpeeds(MecanumDriveWheelSpeeds speeds) { - final double frontLeftFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.frontLeftMetersPerSecond)).in(Volts); - final double frontRightFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.frontRightMetersPerSecond)).in(Volts); - final double backLeftFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.rearLeftMetersPerSecond)).in(Volts); - final double backRightFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.rearRightMetersPerSecond)).in(Volts); + final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeftMetersPerSecond); + final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRightMetersPerSecond); + final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeftMetersPerSecond); + final double backRightFeedforward = m_feedforward.calculate(speeds.rearRightMetersPerSecond); final double frontLeftOutput = m_frontLeftPIDController.calculate( 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 ada2b6d83e0..de481094486 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 @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.mecanumdriveposeestimator; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -110,14 +107,10 @@ public MecanumDriveWheelPositions getCurrentDistances() { * @param speeds The desired wheel speeds. */ public void setSpeeds(MecanumDriveWheelSpeeds speeds) { - final double frontLeftFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.frontLeftMetersPerSecond)).in(Volts); - final double frontRightFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.frontRightMetersPerSecond)).in(Volts); - final double backLeftFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.rearLeftMetersPerSecond)).in(Volts); - final double backRightFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.rearRightMetersPerSecond)).in(Volts); + final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeftMetersPerSecond); + final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRightMetersPerSecond); + final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeftMetersPerSecond); + final double backRightFeedforward = m_feedforward.calculate(speeds.rearRightMetersPerSecond); final double frontLeftOutput = m_frontLeftPIDController.calculate( diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java index 7e9e8e0370a..dbb234a8d0a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems; -import static edu.wpi.first.units.Units.DegreesPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.NotLogged; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -143,9 +140,7 @@ public Command turnToAngleCommand(double angleDeg) { 0, m_controller.calculate(m_gyro.getRotation2d().getDegrees(), angleDeg) // Divide feedforward voltage by battery voltage to normalize it to [-1, 1] - + m_feedforward - .calculate(DegreesPerSecond.of(m_controller.getSetpoint().velocity)) - .in(Volts) + + m_feedforward.calculate(m_controller.getSetpoint().velocity) / RobotController.getBatteryVoltage())) .until(m_controller::atGoal) .finallyDo(() -> m_drive.arcadeDrive(0, 0)); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java index a10f6f91436..39023c2e8ef 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java @@ -4,8 +4,6 @@ package edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems; -import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Volts; import static edu.wpi.first.wpilibj2.command.Commands.parallel; import static edu.wpi.first.wpilibj2.command.Commands.waitUntil; @@ -60,9 +58,7 @@ public Command shootCommand(double setpointRotationsPerSecond) { run( () -> { m_shooterMotor.set( - m_shooterFeedforward - .calculate(RotationsPerSecond.of(setpointRotationsPerSecond)) - .in(Volts) + m_shooterFeedforward.calculate(setpointRotationsPerSecond) + m_shooterFeedback.calculate( m_shooterEncoder.getRate(), setpointRotationsPerSecond)); }), diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java index eb911054a58..d0d1ed4dcdc 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.simpledifferentialdrivesimulation; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; @@ -97,10 +94,8 @@ public Drivetrain() { /** Sets speeds to the drivetrain motors. */ public void setSpeeds(DifferentialDriveWheelSpeeds speeds) { - final double leftFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.leftMetersPerSecond)).in(Volts); - final double rightFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.rightMetersPerSecond)).in(Volts); + final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond); + final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond); double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond); double rightOutput = diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java index eb95c4c52ca..b57ba6e5eae 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java @@ -4,10 +4,6 @@ package edu.wpi.first.wpilibj.examples.swervebot; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -127,10 +123,7 @@ public void setDesiredState(SwerveModuleState desiredState) { final double driveOutput = m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speedMetersPerSecond); - final double driveFeedforward = - m_driveFeedforward - .calculate(MetersPerSecond.of(desiredState.speedMetersPerSecond)) - .in(Volts); + final double driveFeedforward = m_driveFeedforward.calculate(desiredState.speedMetersPerSecond); // Calculate the turning motor output from the turning PID controller. final double turnOutput = @@ -138,9 +131,7 @@ public void setDesiredState(SwerveModuleState desiredState) { m_turningEncoder.getDistance(), desiredState.angle.getRadians()); final double turnFeedforward = - m_turnFeedforward - .calculate(RadiansPerSecond.of(m_turningPIDController.getSetpoint().velocity)) - .in(Volts); + m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity); m_driveMotor.setVoltage(driveOutput + driveFeedforward); m_turningMotor.setVoltage(turnOutput + turnFeedforward); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java index f6a9b46bdd8..e8af157d37b 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java @@ -4,10 +4,6 @@ package edu.wpi.first.wpilibj.examples.swervedriveposeestimator; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -126,10 +122,7 @@ public void setDesiredState(SwerveModuleState desiredState) { final double driveOutput = m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speedMetersPerSecond); - final double driveFeedforward = - m_driveFeedforward - .calculate(MetersPerSecond.of(desiredState.speedMetersPerSecond)) - .in(Volts); + final double driveFeedforward = m_driveFeedforward.calculate(desiredState.speedMetersPerSecond); // Calculate the turning motor output from the turning PID controller. final double turnOutput = @@ -137,9 +130,7 @@ public void setDesiredState(SwerveModuleState desiredState) { m_turningEncoder.getDistance(), desiredState.angle.getRadians()); final double turnFeedforward = - m_turnFeedforward - .calculate(RadiansPerSecond.of(m_turningPIDController.getSetpoint().velocity)) - .in(Volts); + m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity); m_driveMotor.setVoltage(driveOutput + driveFeedforward); m_turningMotor.setVoltage(turnOutput + turnFeedforward); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysidroutine/subsystems/Shooter.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysidroutine/subsystems/Shooter.java index cc7fbac2fe4..affceda1443 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysidroutine/subsystems/Shooter.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysidroutine/subsystems/Shooter.java @@ -95,9 +95,7 @@ public Command runShooter(DoubleSupplier shooterSpeed) { return run(() -> { m_shooterMotor.setVoltage( m_shooterFeedback.calculate(m_shooterEncoder.getRate(), shooterSpeed.getAsDouble()) - + m_shooterFeedforward - .calculate(RotationsPerSecond.of(shooterSpeed.getAsDouble())) - .in(Volts)); + + m_shooterFeedforward.calculate(shooterSpeed.getAsDouble())); m_feederMotor.set(ShooterConstants.kFeederSpeed); }) .finallyDo( diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java index 9356481ffa0..b7603a6bfbe 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java @@ -4,15 +4,8 @@ package edu.wpi.first.math.controller; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.proto.SimpleMotorFeedforwardProto; import edu.wpi.first.math.controller.struct.SimpleMotorFeedforwardStruct; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.PerUnit; -import edu.wpi.first.units.TimeUnit; -import edu.wpi.first.units.Unit; -import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; @@ -136,7 +129,7 @@ public double getDt() { * @param velocity The velocity setpoint. * @param acceleration The acceleration setpoint. * @return The computed feedforward. - * @deprecated Use the current/next velocity overload instead. + * @deprecated Use {@link #calculateWithVelocities(double, double)} instead. */ @SuppressWarnings("removal") @Deprecated(forRemoval = true, since = "2025") @@ -150,50 +143,30 @@ public double calculate(double velocity, double acceleration) { * * @param velocity The velocity setpoint. * @return The computed feedforward. - * @deprecated Use the current/next velocity overload instead. */ - @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 The velocity parameter either as distance or angle. - * @param setpoint The velocity setpoint. - * @return The computed feedforward. - */ - public Voltage calculate(Measure> setpoint) { - return calculate(setpoint, setpoint); - } - /** * Calculates the feedforward from the gains and setpoints assuming discrete control. * *

Note this method is inaccurate when the velocity crosses 0. * - * @param The velocity parameter either as distance or angle. * @param currentVelocity The current velocity setpoint. * @param nextVelocity The next velocity setpoint. * @return The computed feedforward. */ - public Voltage calculate( - Measure> currentVelocity, - Measure> nextVelocity) { + public double calculateWithVelocities(double currentVelocity, double nextVelocity) { // See wpimath/algorithms.md#Simple_motor_feedforward for derivation if (ka == 0.0) { - return Volts.of(ks * Math.signum(nextVelocity.magnitude()) + kv * nextVelocity.magnitude()); + return ks * Math.signum(nextVelocity) + 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( - ks * Math.signum(currentVelocity.magnitude()) - + 1.0 / B_d * (nextVelocity.magnitude() - A_d * currentVelocity.magnitude())); + return ks * Math.signum(currentVelocity) + 1.0 / B_d * (nextVelocity - A_d * currentVelocity); } } diff --git a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h index 932172810df..87099047f5b 100644 --- a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h @@ -84,14 +84,15 @@ class SimpleMotorFeedforward { } /** - * Calculates the feedforward from the gains and setpoint assuming discrete - * control. Use this method when the setpoint does not change. + * Calculates the feedforward from the gains and velocity setpoint assuming + * discrete control. Use this method when the velocity setpoint does not + * change. * - * @param setpoint The velocity setpoint. + * @param velocity The velocity setpoint. * @return The computed feedforward, in volts. */ - constexpr units::volt_t Calculate(units::unit_t setpoint) const { - return Calculate(setpoint, setpoint); + constexpr units::volt_t Calculate(units::unit_t velocity) const { + return Calculate(velocity, velocity); } /** diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java index 1fd6f58d25f..3c0a74835a2 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java @@ -4,7 +4,6 @@ package edu.wpi.first.math.controller; -import static edu.wpi.first.units.Units.RadiansPerSecond; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertThrows; @@ -31,23 +30,23 @@ void testCalculate() { var r = VecBuilder.fill(2.0); var nextR = VecBuilder.fill(3.0); - var currentVelocity = RadiansPerSecond.mutable(2.0); - var nextVelocity = RadiansPerSecond.mutable(3.0); + double currentVelocity = 2.0; // rad/s + double nextVelocity = 3.0; // rad/s assertEquals( 37.52499583432516 + 0.5, - simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(), + simpleMotor.calculateWithVelocities(currentVelocity, nextVelocity), 0.002); assertEquals( plantInversion.calculate(r, nextR).get(0, 0) + Ks, - simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(), + simpleMotor.calculateWithVelocities(currentVelocity, nextVelocity), 0.002); // These won't match exactly. It's just an approximation to make sure they're // in the same ballpark. assertEquals( plantInversion.calculate(r, nextR).get(0, 0) + Ks, - simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(), + simpleMotor.calculateWithVelocities(currentVelocity, nextVelocity), 2.0); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java index 1cedcae1de5..52af40e89b2 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java @@ -4,8 +4,6 @@ package edu.wpi.first.math.trajectory; -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.assertDoesNotThrow; import static org.junit.jupiter.api.Assertions.assertTrue; @@ -53,39 +51,27 @@ void testDifferentialDriveVoltageConstraint() { assertAll( () -> assertTrue( - feedforward - .calculate( - MetersPerSecond.of(wheelSpeeds.leftMetersPerSecond), - MetersPerSecond.of( - wheelSpeeds.leftMetersPerSecond + dt * acceleration)) - .in(Volts) + feedforward.calculateWithVelocities( + wheelSpeeds.leftMetersPerSecond, + wheelSpeeds.leftMetersPerSecond + dt * acceleration) <= maxVoltage + 0.05), () -> assertTrue( - feedforward - .calculate( - MetersPerSecond.of(wheelSpeeds.leftMetersPerSecond), - MetersPerSecond.of( - wheelSpeeds.leftMetersPerSecond + dt * acceleration)) - .in(Volts) + feedforward.calculateWithVelocities( + wheelSpeeds.leftMetersPerSecond, + wheelSpeeds.leftMetersPerSecond + dt * acceleration) >= -maxVoltage - 0.05), () -> assertTrue( - feedforward - .calculate( - MetersPerSecond.of(wheelSpeeds.rightMetersPerSecond), - MetersPerSecond.of( - wheelSpeeds.rightMetersPerSecond + dt * acceleration)) - .in(Volts) + feedforward.calculateWithVelocities( + wheelSpeeds.rightMetersPerSecond, + wheelSpeeds.rightMetersPerSecond + dt * acceleration) <= maxVoltage + 0.05), () -> assertTrue( - feedforward - .calculate( - MetersPerSecond.of(wheelSpeeds.rightMetersPerSecond), - MetersPerSecond.of( - wheelSpeeds.rightMetersPerSecond + dt * acceleration)) - .in(Volts) + feedforward.calculateWithVelocities( + wheelSpeeds.rightMetersPerSecond, + wheelSpeeds.rightMetersPerSecond + dt * acceleration) >= -maxVoltage - 0.05)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java index 80d6abb41cb..2f4ffac2f2a 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java @@ -4,7 +4,6 @@ package edu.wpi.first.math.trajectory; -import static edu.wpi.first.units.Units.RadiansPerSecond; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertNotEquals; @@ -44,11 +43,9 @@ private static void assertNear( private static ExponentialProfile.State checkDynamics( ExponentialProfile profile, ExponentialProfile.State current, ExponentialProfile.State goal) { var next = profile.calculate(kDt, current, goal); - var currentVelocity = RadiansPerSecond.mutable(current.velocity); - var nextVelocity = RadiansPerSecond.mutable(next.velocity); - var signal = feedforward.calculate(currentVelocity, nextVelocity); + var signal = feedforward.calculateWithVelocities(current.velocity, next.velocity); - assertTrue(Math.abs(signal.magnitude()) < constraints.maxInput + 1e-9); + assertTrue(Math.abs(signal) < constraints.maxInput + 1e-9); return next; }