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

Co-authored-by: Tyler Veness <[email protected]>

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

wpimath/algorithms.md

@@ -1,5 +1,219 @@
 # Algorithms
 
+## Simple motor feedforward
+
+### Derivation
+
+For a simple DC motor with the model
+
+```
+  dx/dt = −kᵥ/kₐ x + 1/kₐ u - kₛ/kₐ sgn(x),
+```
+
+where
+
+```
+  A = −kᵥ/kₐ
+  B = 1/kₐ
+  c = -kₛ/kₐ sgn(x)
+  A_d = eᴬᵀ
+  B_d = A⁻¹(eᴬᵀ - I)B
+  dx/dt = Ax + Bu + c
+```
+
+Discretize the affine model.
+
+```
+  dx/dt = Ax + Bu + c
+  dx/dt = Ax + B(u + B⁺c)
+  xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
+  xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
+  xₖ₊₁ = A_d xₖ + B_d uₖ + B_d B⁺cₖ
+```
+
+Solve for uₖ.
+
+```
+  B_d uₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ
+```
+
+Substitute in B assuming sgn(x) is a constant for the duration of the step.
+
+```
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − kₐ(-(kₛ/kₐ sgn(x)))
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kₐ(kₛ/kₐ sgn(x))
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kₛ sgn(x)
+```
+
+Simplify the model when kₐ = 0.
+
+Simplify A.
+
+```
+  A = −kᵥ/kₐ
+```
+
+As kₐ approaches zero, A approaches -∞.
+
+```
+  A = −∞
+```
+
+Simplify A_d.
+
+```
+  A_d = eᴬᵀ
+  A_d = exp(−∞)
+  A_d = 0
+```
+
+Simplify B_d.
+
+```
+  B_d = A⁻¹(eᴬᵀ - I)B
+  B_d = A⁻¹((0) - I)B
+  B_d = A⁻¹(-I)B
+  B_d = -A⁻¹B
+  B_d = -(−kᵥ/kₐ)⁻¹(1/kₐ)
+  B_d = (kᵥ/kₐ)⁻¹(1/kₐ)
+  B_d = kₐ/kᵥ(1/kₐ)
+  B_d = 1/kᵥ
+```
+
+Substitute these into the feedforward equation.
+
+```
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kₛ sgn(x)
+  uₖ = (1/kᵥ)⁺(xₖ₊₁ − (0) xₖ) + kₛ sgn(x)
+  uₖ = (1/kᵥ)⁺(xₖ₊₁) + kₛ sgn(x)
+  uₖ = kᵥxₖ₊₁ + kₛ sgn(x)
+  uₖ = kₛ sgn(x) + kᵥxₖ₊₁
+```
+
+Simplify the model when ka ≠ 0
+
+```
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ)
+```
+
+where
+
+```
+  A = −kᵥ/kₐ
+  B = 1/kₐ
+  A_d = eᴬᵀ
+  B_d = A⁻¹(eᴬᵀ - I)B
+```
+
+## Elevator feedforward
+
+### Derivation
+
+For an elevator with the model
+
+```
+  dx/dt = −kᵥ/kₐ x + 1/kₐ u - kg/kₐ - kₛ/kₐ sgn(x)
+```
+
+where
+
+```
+  A = −kᵥ/kₐ
+  B = 1/kₐ
+  c = -(kg/kₐ + kₛ/kₐ sgn(x))
+  A_d = eᴬᵀ
+  B_d = A⁻¹(eᴬᵀ - I)B
+  dx/dt = Ax + Bu + c
+```
+
+Discretize the affine model.
+
+```
+  dx/dt = Ax + Bu + c
+  dx/dt = Ax + B(u + B⁺c)
+  xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
+  xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
+  xₖ₊₁ = A_d xₖ + B_d uₖ + B_d B⁺cₖ
+```
+
+Solve for uₖ.
+
+```
+  B_d uₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ
+```
+
+Substitute in B assuming sgn(x) is a constant for the duration of the step.
+
+```
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − kₐ(-(kg/kₐ + kₛ/kₐ sgn(x)))
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kₐ(kg/kₐ + kₛ/kₐ sgn(x))
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kg + kₛ sgn(x)
+```
+
+Simplify the model when kₐ = 0.
+
+Simplify A.
+
+```
+  A = −kᵥ/kₐ
+```
+
+As kₐ approaches zero, A approaches -∞.
+
+```
+  A = −∞
+```
+
+Simplify A_d.
+
+```
+  A_d = eᴬᵀ
+  A_d = exp(−∞)
+  A_d = 0
+```
+
+Simplify B_d.
+
+```
+  B_d = A⁻¹(eᴬᵀ - I)B
+  B_d = A⁻¹((0) - I)B
+  B_d = A⁻¹(-I)B
+  B_d = -A⁻¹B
+  B_d = -(−kᵥ/kₐ)⁻¹(1/kₐ)
+  B_d = (kᵥ/kₐ)⁻¹(1/kₐ)
+  B_d = kₐ/kᵥ(1/kₐ)
+  B_d = 1/kᵥ
+```
+
+Substitute these into the feedforward equation.
+
+```
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kg + kₛ sgn(x)
+  uₖ = (1/kᵥ)⁺(xₖ₊₁ − (0) xₖ) + kg + kₛ sgn(x)
+  uₖ = (1/kᵥ)⁺(xₖ₊₁) + kg + kₛ sgn(x)
+  uₖ = kᵥxₖ₊₁ + kg + kₛ sgn(x)
+  uₖ = kₛ sgn(x) + kg + kᵥxₖ₊₁
+```
+
+Simplify the model when ka ≠ 0
+
+```
+  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ)
+```
+
+where
+
+```
+  A = −kᵥ/kₐ
+  B = 1/kₐ
+  A_d = eᴬᵀ
+  B_d = A⁻¹(eᴬᵀ - I)B
+```
+
 ## Closed form Kalman gain for continuous Kalman filter with A = 0 and C = I
 
 ### Derivation