[wpimath] Improve SimpleMotorFeedforward argument names and deprecation message

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<MecanumDriveWheelSpeeds> m_currentWheelSpeeds;
   private final Consumer<MecanumDriveMotorVoltages> m_outputDriveVoltages;
   private final Consumer<MecanumDriveWheelSpeeds> 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));
     }
   }
 

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<Double, Double> 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

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);

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);

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());
   }
 

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;
   }

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);
   }
 }

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

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. */

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(