refactor: Rewrite arm feedforward classes.

src/main/java/frc/lib/TwoJointedArmFeedforward.java → src/main/java/frc/lib/DoubleJointedArmFeedforward.java

@@ -4,7 +4,7 @@
 import org.ejml.data.MatrixType;
 import org.ejml.simple.SimpleMatrix;
 
-public class TwoJointedArmFeedforward {
+public class DoubleJointedArmFeedforward {
 
   private final double m1;
   private final double m2;
@@ -22,7 +22,7 @@ public class TwoJointedArmFeedforward {
   private final SimpleMatrix Kb_MATRIX = new SimpleMatrix(2, 2, MatrixType.DDRM);
   private final SimpleMatrix B_MATRIX = new SimpleMatrix(2, 2, MatrixType.DDRM);
 
-  public TwoJointedArmFeedforward(JointConstants shoulder, JointConstants elbow) {
+  public DoubleJointedArmFeedforward(JointConstants shoulder, JointConstants elbow) {
     this.m1 = shoulder.massKg();
     this.m2 = elbow.massKg();
     this.l1 = shoulder.lengthMeters();

src/main/java/frc/lib/SingleJointedArmFeedforward.java

@@ -1,29 +1,75 @@
 package frc.lib;
 
-import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.geometry.Rotation2d;
 
 public class SingleJointedArmFeedforward {
 
-  private final ArmFeedforward feedforward;
+  public static class SingleJointedArmFeedforwardConstants {
+    public double kS;
+    public double kG;
+    public double kV;
+    public double kA;
 
-  public SingleJointedArmFeedforward(double ks, double kg, double kv) {
-    feedforward = new ArmFeedforward(ks, kg, kv);
+    public SingleJointedArmFeedforwardConstants() {
+      this.kS = 0.0;
+      this.kG = 0.0;
+      this.kV = 0.0;
+      this.kA = 0.0;
+    }
+
+    public SingleJointedArmFeedforwardConstants withKs(double kS) {
+      this.kS = kS;
+      return this;
+    }
+
+    public SingleJointedArmFeedforwardConstants withKg(double kG) {
+      this.kG = kG;
+      return this;
+    }
+
+    /**
+     * Calculates the gravity compensation constant for an arm given the voltage applied at an
+     * equilibrium position.
+     *
+     * @param angle the equilibrium position of the arm.
+     * @param volts the voltage applied to the arm to hold it the equilibrium position.
+     * @return the gravity compensation constant for the arm.
+     */
+    public SingleJointedArmFeedforwardConstants withKg(Rotation2d angle, double volts) {
+      double kG = volts / angle.getCos();
+
+      return this.withKg(kG);
+    }
+
+    public SingleJointedArmFeedforwardConstants withKv(double kV) {
+      this.kV = kV;
+      return this;
+    }
+
+    public SingleJointedArmFeedforwardConstants withKa(double kA) {
+      this.kA = kA;
+      return this;
+    }
   }
 
-  public SingleJointedArmFeedforward(double ks, double kg, double kv, double ka) {
-    feedforward = new ArmFeedforward(ks, kg, kv, ka);
+  private final SingleJointedArmFeedforwardConstants constants;
+
+  public SingleJointedArmFeedforward(SingleJointedArmFeedforwardConstants constants) {
+    this.constants = constants;
   }
 
   public double calculate(Rotation2d position) {
-    return feedforward.calculate(position.getRadians(), 0);
+    return calculate(position, new Rotation2d());
   }
 
-  public double calculate(Rotation2d position, double velocity) {
-    return feedforward.calculate(position.getRadians(), velocity);
+  public double calculate(Rotation2d position, Rotation2d velocity) {
+    return calculate(position, new Rotation2d(), new Rotation2d());
   }
 
-  public double calculate(Rotation2d position, double velocity, double acceleration) {
-    return feedforward.calculate(position.getRadians(), velocity, acceleration);
+  public double calculate(Rotation2d position, Rotation2d velocity, Rotation2d acceleration) {
+    return constants.kS * Math.signum(velocity.getRotations())
+        + constants.kG * position.getCos()
+        + constants.kV * velocity.getRotations()
+        + constants.kA * acceleration.getRotations();
   }
 }

src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java

@@ -8,6 +8,7 @@
 import frc.lib.AccelerationCalculator;
 import frc.lib.Configurator;
 import frc.lib.SingleJointedArmFeedforward;
+import frc.lib.SingleJointedArmFeedforward.SingleJointedArmFeedforwardConstants;
 import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
 
 /** Shoulder motor using a Spark Max. */
@@ -30,7 +31,7 @@ public ShoulderMotorIOSparkMax() {
 
     feedback = new PIDController(ShoulderMotorConstants.KP, 0, 0);
 
-    feedforward = new SingleJointedArmFeedforward(0, 0, 0);
+    feedforward = new SingleJointedArmFeedforward(new SingleJointedArmFeedforwardConstants());
 
     accelerationCalculator = new AccelerationCalculator();
   }
@@ -71,7 +72,8 @@ public void setSetpoint(double positionRotations, double velocityRotationsPerSec
 
     double feedforwardVolts =
         feedforward.calculate(
-            Rotation2d.fromRotations(measuredPositionRotations), velocityRotationsPerSecond);
+            Rotation2d.fromRotations(measuredPositionRotations),
+            Rotation2d.fromRotations(velocityRotationsPerSecond));
 
     setVoltage(feedbackVolts + feedforwardVolts);
   }

src/main/java/frc/robot/arm/WristMotorIOSparkMax.java

@@ -8,6 +8,7 @@
 import frc.lib.AccelerationCalculator;
 import frc.lib.Configurator;
 import frc.lib.SingleJointedArmFeedforward;
+import frc.lib.SingleJointedArmFeedforward.SingleJointedArmFeedforwardConstants;
 import frc.robot.arm.ArmConstants.WristMotorConstants;
 
 public class WristMotorIOSparkMax implements WristMotorIO {
@@ -28,7 +29,7 @@ public WristMotorIOSparkMax() {
 
     feedback = new PIDController(WristMotorConstants.KP, 0, 0);
 
-    feedforward = new SingleJointedArmFeedforward(0, 0, 0);
+    feedforward = new SingleJointedArmFeedforward(new SingleJointedArmFeedforwardConstants());
 
     accelerationCalculator = new AccelerationCalculator();
   }
@@ -71,7 +72,8 @@ public void setSetpoint(double positionRotations, double velocityRotationsPerSec
 
     double feedforwardVolts =
         feedforward.calculate(
-            Rotation2d.fromRotations(measuredPositionRotations), velocityRotationsPerSecond);
+            Rotation2d.fromRotations(measuredPositionRotations),
+            Rotation2d.fromRotations(velocityRotationsPerSecond));
 
     sparkMax.setVoltage(feedbackVolts + feedforwardVolts);
   }

src/main/java/frc/robot/intake/PivotMotorIOTalonSRX.java

@@ -5,9 +5,9 @@
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
-import frc.lib.ArmFeedforwardCalculator;
 import frc.lib.Configurator;
 import frc.lib.SingleJointedArmFeedforward;
+import frc.lib.SingleJointedArmFeedforward.SingleJointedArmFeedforwardConstants;
 import frc.robot.intake.IntakeConstants.PivotMotorConstants;
 
 /** Pivot motor using a Talon SRX. */
@@ -25,11 +25,10 @@ public PivotMotorIOTalonSRX() {
 
     feedback = new PIDController(PivotMotorConstants.KP, 0, 0);
 
-    // TODO
-    double kg =
-        ArmFeedforwardCalculator.calculateArmGravityCompensation(Rotation2d.fromDegrees(26), 1.8);
+    SingleJointedArmFeedforwardConstants constants =
+        new SingleJointedArmFeedforwardConstants().withKg(Rotation2d.fromDegrees((26)), 1.8);
 
-    feedforward = new SingleJointedArmFeedforward(0, kg, 0);
+    feedforward = new SingleJointedArmFeedforward(constants);
   }
 
   @Override
@@ -82,8 +81,7 @@ public void setSetpoint(double positionRotations, double velocityRotationsPerSec
     double feedbackVolts = feedback.calculate(measuredPositionRotations, positionRotations);
 
     double feedforwardVolts =
-        feedforward.calculate(
-            Rotation2d.fromRotations(measuredPositionRotations), velocityRotationsPerSecond);
+        feedforward.calculate(Rotation2d.fromRotations(measuredPositionRotations));
 
     double percent = (feedbackVolts + feedforwardVolts) / talonSRX.getBusVoltage();