From 17b17f770b2a9546a08c9086c108e0eaeb670200 Mon Sep 17 00:00:00 2001
From: Hayden Heroux <haydenheroux@gmail.com>
Date: Thu, 22 Feb 2024 19:17:37 -0500
Subject: [PATCH] refactor: Add single jointed arm feedforward.

---
 .../frc/lib/SingleJointedArmFeedforward.java  | 30 +++++++++++++++++++
 .../robot/arm/ShoulderMotorIOSparkMax.java    |  8 ++---
 .../frc/robot/intake/PivotMotorIOSim.java     |  8 +++--
 3 files changed, 39 insertions(+), 7 deletions(-)
 create mode 100644 src/main/java/frc/lib/SingleJointedArmFeedforward.java

diff --git a/src/main/java/frc/lib/SingleJointedArmFeedforward.java b/src/main/java/frc/lib/SingleJointedArmFeedforward.java
new file mode 100644
index 0000000..303f1f3
--- /dev/null
+++ b/src/main/java/frc/lib/SingleJointedArmFeedforward.java
@@ -0,0 +1,30 @@
+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 SingleJointedArmFeedforward(double ks, double kg, double kv) {
+        feedforward = new ArmFeedforward(ks, kg, kv);
+    }
+
+    public SingleJointedArmFeedforward(double ks, double kg, double kv, double ka) {
+        feedforward = new ArmFeedforward(ks, kg, kv, ka);
+    }
+
+    public double calculate(Rotation2d position) {
+        return feedforward.calculate(position.getRadians(), 0);
+    }
+
+    public double calculate(Rotation2d position, double velocity) {
+        return feedforward.calculate(position.getRadians(), velocity);
+    }
+
+    public double calculate(Rotation2d position, double velocity, double acceleration) {
+        return feedforward.calculate(position.getRadians(), velocity, acceleration);
+    }
+
+}
diff --git a/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java b/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java
index 23c6886..c9803d5 100644
--- a/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java
+++ b/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java
@@ -3,12 +3,12 @@
 import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
-import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
 import frc.lib.AccelerationCalculator;
 import frc.lib.ArmFeedforwardCalculator;
 import frc.lib.Configurator;
+import frc.lib.SingleJointedArmFeedforward;
 import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
 
 /** Shoulder motor using a Spark Max. */
@@ -21,7 +21,7 @@ public class ShoulderMotorIOSparkMax implements ShoulderMotorIO {
   private final PIDController feedback;
 
   /** Feedforward controller for shoulder position. */
-  private final ArmFeedforward feedforward;
+  private final SingleJointedArmFeedforward feedforward;
 
   private final AccelerationCalculator accelerationCalculator;
 
@@ -32,7 +32,7 @@ public ShoulderMotorIOSparkMax() {
     feedback = new PIDController(ShoulderMotorConstants.KP, 0, 0);
 
     feedforward =
-        new ArmFeedforward(
+        new SingleJointedArmFeedforward(
             0,
             ArmFeedforwardCalculator.calculateArmGravityCompensation(
                 Rotation2d.fromDegrees(18.0), 0.1222),
@@ -74,7 +74,7 @@ public void setSetpoint(double positionRotations, double velocityRotationsPerSec
     double feedbackVolts = feedback.calculate(measuredPositionRotations, positionRotations);
 
     double feedforwardVolts =
-        feedforward.calculate(measuredPositionRotations, velocityRotationsPerSecond);
+        feedforward.calculate(Rotation2d.fromRotations(measuredPositionRotations), velocityRotationsPerSecond);
 
     sparkMax.setVoltage(feedbackVolts + feedforwardVolts);
   }
diff --git a/src/main/java/frc/robot/intake/PivotMotorIOSim.java b/src/main/java/frc/robot/intake/PivotMotorIOSim.java
index 52eb5a7..0eb7a54 100644
--- a/src/main/java/frc/robot/intake/PivotMotorIOSim.java
+++ b/src/main/java/frc/robot/intake/PivotMotorIOSim.java
@@ -2,9 +2,11 @@
 
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import frc.lib.SingleJointedArmFeedforward;
 import frc.robot.RobotConstants;
 import frc.robot.intake.IntakeConstants.PivotMotorConstants;
 
@@ -17,7 +19,7 @@ public class PivotMotorIOSim implements PivotMotorIO {
 
   private final PIDController feedback;
 
-  private final ArmFeedforward feedforward;
+  private final SingleJointedArmFeedforward feedforward;
 
   public PivotMotorIOSim() {
     motor = DCMotor.getVex775Pro(1);
@@ -35,7 +37,7 @@ public PivotMotorIOSim() {
 
     feedback = new PIDController(PivotMotorConstants.KP, 0, 0);
 
-    feedforward = new ArmFeedforward(0, 0, 0);
+    feedforward = new SingleJointedArmFeedforward(0, 0, 0);
   }
 
   @Override
@@ -61,7 +63,7 @@ public void setSetpoint(double positionRotations, double velocityRotationsPerSec
 
     double feedforwardVolts =
         feedforward.calculate(
-            Units.rotationsToRadians(measuredPositionRotations), velocityRotationsPerSecond);
+            Rotation2d.fromRotations(measuredPositionRotations), velocityRotationsPerSecond);
 
     singleJointedArmSim.setInputVoltage(feedbackVolts + feedforwardVolts);
   }