[wpimath] Fully discretized ElevatorFF and ArmFF (#7024)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java

@@ -4,6 +4,10 @@
 
 package edu.wpi.first.wpilibj.examples.armbot.subsystems;
 
+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.ArmFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
@@ -41,7 +45,10 @@ public class ArmSubsystem extends ProfiledPIDSubsystem {
   @Override
   public void useOutput(double output, TrapezoidProfile.State setpoint) {
     // Calculate the feedforward from the sepoint
-    double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity);
+    double feedforward =
+        m_feedforward
+            .calculate(Radians.of(setpoint.position), RadiansPerSecond.of(setpoint.velocity))
+            .in(Volts);
     // Add the feedforward to the PID output to get the motor output
     m_motor.setVoltage(output + feedforward);
   }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java

@@ -4,6 +4,10 @@
 
 package edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems;
 
+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.ArmFeedforward;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants;
@@ -33,7 +37,10 @@ public class ArmSubsystem extends TrapezoidProfileSubsystem {
   @Override
   public void useState(TrapezoidProfile.State setpoint) {
     // Calculate the feedforward from the sepoint
-    double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity);
+    double feedforward =
+        m_feedforward
+            .calculate(Radians.of(setpoint.position), RadiansPerSecond.of(setpoint.velocity))
+            .in(Volts);
     // Add the feedforward to the PID output to get the motor output
     m_motor.setSetpoint(
         ExampleSmartMotorController.PIDMode.kPosition, setpoint.position, feedforward / 12.0);

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java

@@ -4,6 +4,9 @@
 
 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;
@@ -110,7 +113,10 @@ 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(m_setpoint.velocity, next.velocity, 0.020);
+    double feedforwardOutput =
+        m_feedforward
+            .calculate(MetersPerSecond.of(m_setpoint.velocity), MetersPerSecond.of(next.velocity))
+            .in(Volts);
 
     m_motor.setVoltage(pidOutput + feedforwardOutput);
 

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java

@@ -4,6 +4,9 @@
 
 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;
@@ -51,6 +54,8 @@ public class Robot extends TimedRobot {
     // Run controller and update motor output
     m_motor.setVoltage(
         m_controller.calculate(m_encoder.getDistance())
-            + m_feedforward.calculate(m_controller.getSetpoint().velocity));
+            + m_feedforward
+                .calculate(MetersPerSecond.of(m_controller.getSetpoint().velocity))
+                .in(Volts));
   }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java

@@ -4,6 +4,9 @@
 
 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;
@@ -101,7 +104,8 @@ 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(m_controller.getSetpoint().velocity);
+    double feedforwardOutput =
+        m_feedforward.calculate(MetersPerSecond.of(m_controller.getSetpoint().velocity)).in(Volts);
     m_motor.setVoltage(pidOutput + feedforwardOutput);
   }