[wpimath] fully discretized ElevatorFF and ArmFF

wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/cpp/subsystems/Elevator.cpp

@@ -48,7 +48,7 @@ void Elevator::ReachGoal(units::meter_t goal) {
   auto pidOutput = m_controller.Calculate(m_encoder.GetDistance(),
                                           m_setpoint.position / 1_m);
   auto feedforwardOutput =
-      m_feedforward.Calculate(m_setpoint.velocity, next.velocity, 20_ms);
+      m_feedforward.Calculate(m_setpoint.velocity, next.velocity);
 
   m_motor.SetVoltage(units::volt_t{pidOutput} + feedforwardOutput);
 

wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h

@@ -7,6 +7,7 @@
 #include <numbers>
 
 #include <units/angle.h>
+#include <units/angular_velocity.h>
 #include <units/time.h>
 #include <units/voltage.h>
 

wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h

@@ -28,5 +28,5 @@ class ShooterSubsystem : public frc2::PIDSubsystem {
   frc::PWMSparkMax m_shooterMotor;
   frc::PWMSparkMax m_feederMotor;
   frc::Encoder m_shooterEncoder;
-  frc::SimpleMotorFeedforward<units::turns> m_shooterFeedforward;
+  frc::SimpleMotorFeedforward<units::radians> m_shooterFeedforward;
 };

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Shooter.h

@@ -38,7 +38,7 @@ class Shooter : public frc2::SubsystemBase {
   frc::Encoder m_shooterEncoder{ShooterConstants::kEncoderPorts[0],
                                 ShooterConstants::kEncoderPorts[1],
                                 ShooterConstants::kEncoderReversed};
-  frc::SimpleMotorFeedforward<units::turns> m_shooterFeedforward{
+  frc::SimpleMotorFeedforward<units::radians> m_shooterFeedforward{
       ShooterConstants::kS, ShooterConstants::kV};
   frc::PIDController m_shooterFeedback{ShooterConstants::kP, 0.0, 0.0};
 };

wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h

@@ -60,7 +60,7 @@ inline constexpr units::turns_per_second_t kShooterTolerance = 50_tps;
 inline constexpr double kP = 1.0;
 
 inline constexpr units::volt_t kS = 0.05_V;
-inline constexpr kv_unit_t kV = (12_V) / kShooterFreeSpeed;
+inline constexpr kv_unit_t kV = 12_V / kShooterFreeSpeed;
 inline constexpr ka_unit_t kA = 0_V * 1_s * 1_s / 1_tr;
 
 inline constexpr double kFeederSpeed = 0.5;

wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Shooter.h

@@ -48,6 +48,6 @@ class Shooter : public frc2::SubsystemBase {
           },
           this}};
   frc::PIDController m_shooterFeedback{constants::shooter::kP, 0, 0};
-  frc::SimpleMotorFeedforward<units::turns> m_shooterFeedforward{
+  frc::SimpleMotorFeedforward<units::radians> m_shooterFeedforward{
       constants::shooter::kS, constants::shooter::kV, constants::shooter::kA};
 };

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 ArmSubsystem() {
   @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 ArmSubsystem() {
   @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 void reachGoal(double goal) {
 
     // 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 void teleopPeriodic() {
     // 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 void reachGoal(double goal) {
 
     // 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);
   }