create PID helper class for running PID loops in a Mechanism's run method

src/main/java/com/team766/controllers/PIDRunner.java

@@ -0,0 +1,113 @@
+package com.team766.controllers;
+
+import com.team766.framework.Mechanism;
+import com.team766.hal.MotorController;
+import com.team766.library.ValueProvider;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.util.function.Supplier;
+
+/**
+ * Helper class that can set a setpoint in a PID loop, eg within {@link Mechanism.run}.
+ *
+ * Uses functional interfaces for getting the setpoint, output, PID slot, and feedforward.
+ * Can use with lambdas and/or method references, eg:
+ *
+ * PIDRunner pidRunner = new PIDRunner("SHOULDER", leftMotor, ControlMode.Position,
+ *                                     this::getTargetRotations, this::getRotations);
+ */
+public class PIDRunner {
+
+    public static final Supplier<Integer> DEFAULT_SLOT_PICKER = () -> 0;
+    public static final Supplier<Double> NO_ARBITRARY_FEED_FORWARD = () -> 0.0;
+
+    /**
+     * Returns a fixed arbitrary FeedForward supplier, simply returning the latest input ffGain from a config file.
+     * @param arbitraryFFGain Input FeedForward Gain, read from a config file.
+     * @return Fixed FeedForward Gain from the current value of ffGain.
+     */
+    public static final Supplier<Double> fixedArbitraryFeedForward(
+            ValueProvider<Double> arbitraryFFGain) {
+        return () -> arbitraryFFGain.valueOr(0.0);
+    }
+
+    /**
+     * Returns a FeedForward supplier that gets the latest input ffGain from a config file and returns a proportional
+     * FeedForward based on the cosine of the supplied angle.  Useful for arm type mechanisms, where we want to
+     * counteract gravity proportionally to the arm's current angle, where 0 is parallel to the ground and 90
+     * is perpendicular & up.
+     *
+     * @param arbitraryFFGain Input FeedForward Gain, read from a config file.
+     * @param angle Current angle of the mechanism.  0 is parallel to the ground, 90 is perpendicular & up.
+     * @return Proportional FeedForward Gain based on the angle.
+     */
+    public static final Supplier<Double> cosineArbitraryFeedForward(
+            ValueProvider<Double> arbitraryFFGain, Supplier<Double> angle) {
+        return () -> arbitraryFFGain.valueOr(0.0) * Math.cos(Math.toRadians(angle.get()));
+    }
+
+    private final String label;
+    private final MotorController motor;
+    private final MotorController.ControlMode mode;
+    private final Supplier<Double> setPoint;
+    private final Supplier<Double> output;
+    private final Supplier<Integer> slot;
+    private final Supplier<Double> arbitraryFeedForward;
+    private boolean first = true;
+    private double prevSetPoint = 0.0;
+    private double prevArbitraryFeedForward = 0.0;
+    private int prevSlot = 0;
+
+    public PIDRunner(
+            String label,
+            MotorController motor,
+            MotorController.ControlMode mode,
+            Supplier<Double> setPoint,
+            Supplier<Double> output) {
+        this(label, motor, mode, setPoint, output, DEFAULT_SLOT_PICKER, NO_ARBITRARY_FEED_FORWARD);
+    }
+
+    public PIDRunner(
+            String label,
+            MotorController motor,
+            MotorController.ControlMode mode,
+            Supplier<Double> setPoint,
+            Supplier<Double> output,
+            Supplier<Integer> slot,
+            Supplier<Double> arbitraryFeedForward) {
+        this.label = label;
+        this.motor = motor;
+        this.mode = mode;
+        this.setPoint = setPoint;
+        this.output = output;
+        this.slot = slot;
+        this.arbitraryFeedForward = arbitraryFeedForward;
+    }
+
+    public void run() {
+        double currentSetPoint = setPoint.get();
+        double currentOutput = output.get();
+        double currentArbitraryFeedForward = arbitraryFeedForward.get();
+        int currentSlot = slot.get();
+
+        // if we haven't set the setpoint yet, or if the setpoint, feedforward, or slot have
+        // changed, set the setpoint.
+        if (first
+                || prevSetPoint != currentSetPoint
+                || prevArbitraryFeedForward != currentArbitraryFeedForward
+                || currentSlot != prevSlot) {
+            first = false;
+            prevSetPoint = currentSetPoint;
+            prevArbitraryFeedForward = currentArbitraryFeedForward;
+            prevSlot = currentSlot;
+
+            // log to SmartDashboard - useful for PID tuning.
+            SmartDashboard.putNumber(label + " setpoint", currentSetPoint);
+            SmartDashboard.putNumber(label + " output", currentOutput);
+            SmartDashboard.putNumber(label + " PID slot", currentSlot);
+
+            // FIXME: switch to supporting slot, feedForward once that PR is integrated
+            // motor.set(mode, currentSetPoint, slot.get(), arbitraryFeedForward.get());
+            motor.set(mode, currentSetPoint);
+        }
+    }
+}

src/main/java/com/team766/robot/reva/constants/ConfigConstants.java

@@ -10,6 +10,8 @@ private ConfigConstants() {}
 
     public static final String SHOULDER_RIGHT = "shoulder.rightMotor";
     public static final String SHOULDER_LEFT = "shoulder.leftMotor";
+    // TODO: change this to shoulder.arbitraryFFGain
+    public static final String SHOULDER_ARBITRARY_FFGAIN = "shoulder.leftMotor.ffGain";
 
     // intake config values
     public static final String INTAKE_MOTOR = "intake.motor";

src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java

@@ -1,12 +1,12 @@
 package com.team766.robot.reva.mechanisms;
 
+import static com.team766.robot.reva.constants.ConfigConstants.SHOULDER_ARBITRARY_FFGAIN;
 import static com.team766.robot.reva.constants.ConfigConstants.SHOULDER_LEFT;
 import static com.team766.robot.reva.constants.ConfigConstants.SHOULDER_RIGHT;
 
 import com.ctre.phoenix.motorcontrol.NeutralMode;
-import com.ctre.phoenix6.controls.PositionDutyCycle;
-import com.ctre.phoenix6.hardware.TalonFX;
 import com.team766.config.ConfigFileReader;
+import com.team766.controllers.PIDRunner;
 import com.team766.framework.Mechanism;
 import com.team766.hal.MotorController;
 import com.team766.hal.RobotProvider;
@@ -36,10 +36,11 @@ public double getAngle() {
 
     private static final double NUDGE_AMOUNT = 30; // degrees
 
-    private MotorController leftMotor;
-    private MotorController rightMotor;
+    private final MotorController leftMotor;
+    private final MotorController rightMotor;
+    private final PIDRunner pidRunner;
 
-    private ValueProvider<Double> ffGain;
+    private ValueProvider<Double> arbitraryFFGain;
     private double targetRotations = 0.0;
 
     public Shoulder() {
@@ -48,8 +49,18 @@ public Shoulder() {
         rightMotor = RobotProvider.instance.getMotor(SHOULDER_RIGHT);
         rightMotor.follow(leftMotor);
         leftMotor.setNeutralMode(NeutralMode.Brake);
-        ffGain = ConfigFileReader.getInstance().getDouble("shoulder.leftMotor.ffGain");
+        arbitraryFFGain = ConfigFileReader.getInstance().getDouble(SHOULDER_ARBITRARY_FFGAIN);
         leftMotor.setSensorPosition(0);
+
+        pidRunner =
+                new PIDRunner(
+                        "SHOULDER",
+                        leftMotor,
+                        MotorController.ControlMode.Position,
+                        this::getTargetRotations,
+                        this::getRotations,
+                        PIDRunner.DEFAULT_SLOT_PICKER,
+                        PIDRunner.cosineArbitraryFeedForward(arbitraryFFGain, this::getAngle));
     }
 
     public void stop() {
@@ -73,6 +84,10 @@ public double getRotations() {
         return leftMotor.getSensorPosition();
     }
 
+    public double getTargetRotations() {
+        return targetRotations;
+    }
+
     public double getAngle() {
         return rotationsToDegrees(leftMotor.getSensorPosition());
     }
@@ -103,17 +118,10 @@ public void rotate(double angle) {
 
     @Override
     public void run() {
-        SmartDashboard.putNumber("[SHOULDER] Angle", getAngle());
-        SmartDashboard.putNumber("[SHOULDER] Rotations", getRotations());
-        SmartDashboard.putNumber("[SHOULDER] Target Rotations", targetRotations);
-
-        TalonFX leftTalon = (TalonFX) leftMotor;
-        SmartDashboard.putNumber("[SHOULDER] ffGain", ffGain.get());
-        double ff = ffGain.valueOr(0.0) * Math.cos(Math.toRadians(getAngle()));
-        SmartDashboard.putNumber("[SHOULDER] FF", ff);
+        pidRunner.run();
+        // also log velocity, for PID tuning
+        // TODO: consider moving this into PIDRunner
         SmartDashboard.putNumber("[SHOULDER VELOCITY]", Math.abs(leftMotor.getSensorVelocity()));
-        PositionDutyCycle positionRequest = new PositionDutyCycle(targetRotations);
-        positionRequest.FeedForward = ff;
-        leftTalon.setControl(positionRequest);
+        SmartDashboard.putNumber("[SHOULDER ANGLE]", getAngle());
     }
 }