Change how feedforward works (#71)

* Restructure feedforward to MotorFeedforward, add arm and elevator ff

* spotless apply

src/com/stuypulse/stuylib/control/angle/feedforward/AngleArmFeedforward.java

@@ -0,0 +1,46 @@
+/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved. */
+/* This work is licensed under the terms of the MIT license */
+/* found in the root directory of this project. */
+
+package com.stuypulse.stuylib.control.angle.feedforward;
+
+import com.stuypulse.stuylib.control.angle.AngleController;
+import com.stuypulse.stuylib.math.Angle;
+
+/**
+ * A feedforward term to account for gravity for motorized arms that can move continuously (if not
+ * use `ArmFeedforward`)
+ *
+ * <p>The motor feedforward used in the context of an arm will not account for gravity that is
+ * acting on the arm.
+ *
+ * <p>Can be paired with MotorFeedforward or other controllers with .add
+ *
+ * @author Myles Pasetsky (myles.pasetsky@gmail.com)
+ */
+public class AngleArmFeedforward extends AngleController {
+
+    /** voltage to hold arm horizontal */
+    private final Number kG;
+
+    /**
+     * Create arm feedforward
+     *
+     * @param kG term to hold arm vertical against gravity (volts)
+     */
+    public AngleArmFeedforward(Number kG) {
+        this.kG = kG;
+    }
+
+    /**
+     * Calculates voltage to hold arm at the setpoint angle
+     *
+     * @param setpoint setpoint
+     * @param measurement measurement
+     * @return kG * cos(setpoint)
+     */
+    @Override
+    protected double calculate(Angle setpoint, Angle measurement) {
+        return kG.doubleValue() * setpoint.cos();
+    }
+}

src/com/stuypulse/stuylib/control/angle/feedforward/AnglePositionFeedforwardController.java

@@ -5,7 +5,7 @@
 package com.stuypulse.stuylib.control.angle.feedforward;
 
 import com.stuypulse.stuylib.control.angle.AngleController;
-import com.stuypulse.stuylib.control.feedforward.Feedforward;
+import com.stuypulse.stuylib.control.feedforward.MotorFeedforward;
 import com.stuypulse.stuylib.math.Angle;
 import com.stuypulse.stuylib.util.AngleVelocity;
 
@@ -18,7 +18,7 @@
 public class AnglePositionFeedforwardController extends AngleController {
 
     /** the feedforward model */
-    private final Feedforward mFeedforward;
+    private final MotorFeedforward mFeedforward;
 
     /** find the derivative of angular setpoints */
     private final AngleVelocity mDerivative;
@@ -28,7 +28,7 @@ public class AnglePositionFeedforwardController extends AngleController {
      *
      * @param feedforward model
      */
-    public AnglePositionFeedforwardController(Feedforward feedforward) {
+    public AnglePositionFeedforwardController(MotorFeedforward feedforward) {
         mFeedforward = feedforward;
         mDerivative = new AngleVelocity();
     }

src/com/stuypulse/stuylib/control/feedforward/ArmFeedforward.java

@@ -0,0 +1,61 @@
+/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved. */
+/* This work is licensed under the terms of the MIT license */
+/* found in the root directory of this project. */
+
+package com.stuypulse.stuylib.control.feedforward;
+
+import com.stuypulse.stuylib.control.Controller;
+import com.stuypulse.stuylib.streams.filters.IFilter;
+
+/**
+ * A feedforward term to account for gravity for motorized arms.
+ *
+ * <p>The motor feedforward used in the context of an arm will not account for gravity that is
+ * acting on the arm.
+ *
+ * <p>Can be paired with MotorFeedforward or other controllers with .add
+ *
+ * @author Myles Pasetsky (myles.pasetsky@gmail.com)
+ */
+public class ArmFeedforward extends Controller {
+
+    /** voltage to hold arm horizontal */
+    private final Number kG;
+
+    /**
+     * function to configure units of cosine (or even use sin if angles are measured differently)
+     */
+    private final IFilter cosine;
+
+    /**
+     * Create arm feedforward
+     *
+     * @param kG term to hold arm vertical against gravity (volts)
+     */
+    public ArmFeedforward(Number kG) {
+        this(kG, x -> Math.cos(Math.toRadians(x)));
+    }
+
+    /**
+     * Create arm feedforward
+     *
+     * @param kG term to hold arm vertical against gravity (volts)
+     * @param cosine function to calculate cosine of setpoint
+     */
+    public ArmFeedforward(Number kG, IFilter cosine) {
+        this.kG = kG;
+        this.cosine = cosine;
+    }
+
+    /**
+     * Calculates voltage to hold arm at the setpoint angle
+     *
+     * @param setpoint setpoint
+     * @param measurement measurement
+     * @return kG * cos(setpoint)
+     */
+    @Override
+    protected double calculate(double setpoint, double measurement) {
+        return kG.doubleValue() * cosine.get(setpoint);
+    }
+}

src/com/stuypulse/stuylib/control/feedforward/ElevatorFeedforward.java

@@ -0,0 +1,44 @@
+/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved. */
+/* This work is licensed under the terms of the MIT license */
+/* found in the root directory of this project. */
+
+package com.stuypulse.stuylib.control.feedforward;
+
+import com.stuypulse.stuylib.control.Controller;
+
+/**
+ * A feedforward term to account for gravity for motorized lifts.
+ *
+ * <p>The motor feedforward used in the context of a lift will not account for gravity that is
+ * acting on the lift.
+ *
+ * <p>Can be paired with MotorFeedforward or other controllers with .add
+ *
+ * @author Myles Pasetsky (myles.pasetsky@gmail.com)
+ */
+public class ElevatorFeedforward extends Controller {
+
+    /** voltage to hold arm in place against gravity */