Switch to TunableNumbers for gains

Signed-off-by: Jade Turner <spacey-sooty@proton.me>

Author: spacey-sooty

src/main/java/org/curtinfrc/frc2025/Constants.java

@@ -17,6 +17,7 @@
  */
 public final class Constants {
   public static final RobotType robotType = RobotType.DEVBOT;
+  public static final boolean tuningMode = true;
   public static final double ROBOT_X = 660; // mm
   public static final double ROBOT_Y = 680;
 

src/main/java/org/curtinfrc/frc2025/subsystems/ejector/Ejector.java

@@ -1,22 +1,54 @@
 package org.curtinfrc.frc2025.subsystems.ejector;
 
-import static org.curtinfrc.frc2025.subsystems.ejector.EjectorConstants.*;
-
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import org.curtinfrc.frc2025.Constants;
+import org.curtinfrc.frc2025.util.LoggedTunableNumber;
 import org.littletonrobotics.junction.Logger;
 
 public class Ejector extends SubsystemBase {
   private final EjectorIO io;
   private final EjectorIOInputsAutoLogged inputs = new EjectorIOInputsAutoLogged();
-  private final PIDController pid = new PIDController(kP, 0, kD);
-  private final SimpleMotorFeedforward ff = new SimpleMotorFeedforward(kS, kV, kA);
+  private final PIDController pid = new PIDController(0, 0, 0);
+  private SimpleMotorFeedforward ff;
+
+  // Tunable Numbers
+  private final LoggedTunableNumber kS = new LoggedTunableNumber("Ejector/kS");
+  private final LoggedTunableNumber kV = new LoggedTunableNumber("Ejector/kV");
+  private final LoggedTunableNumber kP = new LoggedTunableNumber("Ejector/kP");
+  private final LoggedTunableNumber kD = new LoggedTunableNumber("Ejector/kD");
 
   public Ejector(EjectorIO io) {
     this.io = io;
+    switch (Constants.robotType) {
+      case COMPBOT:
+        kS.initDefault(0.8);
+        kV.initDefault(0.11);
+        ff = new SimpleMotorFeedforward(kS.get(), kV.get());
+        kP.initDefault(0);
+        kD.initDefault(0);
+        pid.setPID(kP.get(), 0, kD.get());
+        break;
+      case DEVBOT:
+        kS.initDefault(0.8);
+        kV.initDefault(0.11);
+        ff = new SimpleMotorFeedforward(kS.get(), kV.get());
+        kP.initDefault(0);
+        kD.initDefault(0);
+        pid.setPID(kP.get(), 0, kD.get());
+        break;
+      case SIMBOT:
+        kS.initDefault(0.8);
+        kV.initDefault(0.11);
+        ff = new SimpleMotorFeedforward(kS.get(), kV.get());
+        kP.initDefault(0);
+        kD.initDefault(0);
+        pid.setPID(kP.get(), 0, kD.get());
+        break;
+    }
   }
 
   public final Trigger sensor = new Trigger(() -> inputs.sensor);
@@ -26,6 +58,14 @@ public Ejector(EjectorIO io) {
   public void periodic() {
     io.updateInputs(inputs);
     Logger.processInputs("Ejector", inputs);
+
+    if (kS.hasChanged(hashCode()) || kD.hasChanged(hashCode())) {
+      ff = new SimpleMotorFeedforward(kS.get(), kV.get());
+    }
+
+    if (kP.hasChanged(hashCode()) || kD.hasChanged(hashCode())) {
+      pid.setPID(kP.get(), 0, kD.get());
+    }
   }
 
   public Command stop() {

src/main/java/org/curtinfrc/frc2025/subsystems/ejector/EjectorConstants.java

@@ -6,9 +6,4 @@ public class EjectorConstants {
   public static final int currentLimit = 60;
   public static final double ejectorMoi = 0.025;
   public static final double ejectorReduction = 1;
-  public static final double kP = 0;
-  public static final double kD = 0;
-  public static final double kS = 0.8;
-  public static final double kV = 0.11;
-  public static final double kA = 0;
 }

src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java

@@ -1,29 +1,60 @@
 package org.curtinfrc.frc2025.subsystems.elevator;
 
-import static org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.*;
+import static org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.pulleyRadiusMeters;
+import static org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.tolerance;
 
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import org.curtinfrc.frc2025.Constants;
 import org.curtinfrc.frc2025.Constants.Setpoints;
+import org.curtinfrc.frc2025.util.LoggedTunableNumber;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
 public class Elevator extends SubsystemBase {
   private final ElevatorIO io;
   private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged();
-  private final PIDController pid = new PIDController(kP, 0, kD);
+  private final PIDController pid = new PIDController(0, 0, 0);
+
+  @AutoLogOutput(key = "Elevator/Setpoint")
   private Setpoints setpoint = Setpoints.COLLECT;
 
+  // Tunable Numbers
+  LoggedTunableNumber kP = new LoggedTunableNumber("Elevator/kP");
+  LoggedTunableNumber kD = new LoggedTunableNumber("Elevator/kD");
+
+  @AutoLogOutput(key = "Elevator/IsNotAtCollect")
   public final Trigger isNotAtCollect = new Trigger(() -> setpoint != Setpoints.COLLECT);
+
   public final Trigger toZero = new Trigger(() -> inputs.touchSensor);
 
+  @AutoLogOutput(key = "Elevator/AtSetpoint")
+  public Trigger atSetpoint = new Trigger(pid::atSetpoint);
+
   public Elevator(ElevatorIO io) {
     this.io = io;
     pid.setTolerance(tolerance);
+    switch (Constants.robotType) {
+      case COMPBOT:
+        kP.initDefault(16);
+        kD.initDefault(0);
+        pid.setPID(kP.get(), 0, kD.get());
+        break;
+      case DEVBOT:
+        kP.initDefault(16);
+        kD.initDefault(0);
+        pid.setPID(kP.get(), 0, kD.get());
+        break;
+      case SIMBOT:
+        kP.initDefault(16);
+        kD.initDefault(0);
+        pid.setPID(kP.get(), 0, kD.get());
+        break;
+    }
   }
 
   @Override
@@ -34,9 +65,11 @@ public void periodic() {
     Logger.recordOutput("Elevator/setpoint", setpoint);
     Logger.recordOutput("Elevator/AtSetpoint", atSetpoint.getAsBoolean());
     Logger.recordOutput("Elevator/ActualError", pid.getError());
-  }
 
-  public Trigger atSetpoint = new Trigger(pid::atSetpoint);
+    if (kP.hasChanged(hashCode()) || kD.hasChanged(hashCode())) {
+      pid.setPID(kP.get(), 0, kD.get());
+    }
+  }
 
   public Command goToSetpoint(Setpoints point) {
     return run(

src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorConstants.java

@@ -5,12 +5,11 @@ public class ElevatorConstants {
   public static int motorPort = 31;
 
   // TODO: TUNE PID and ff
-  public static double tolerance = 0.05;
-  public static double kP = 16;
-  public static double kD = 0;
+  // TODO: not using tunables for this yet
   public static double kV = 0;
   public static double kA = 0.01;
   public static double kG = 0;
+  public static double tolerance = 1;
 
   public static final double pulleyRadiusMeters = 0.002927789;
   public static final double maxHeightMeters = 0.8;

src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIOSim.java

@@ -1,7 +1,5 @@
 package org.curtinfrc.frc2025.subsystems.elevator;
 
-import static org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.kA;
-import static org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.kV;
 import static org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.maxHeightMeters;
 import static org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.positionMetresToRotations;
 
@@ -10,7 +8,7 @@
 
 public class ElevatorIOSim implements ElevatorIO {
   private final ElevatorSim elevatorSim =
-      new ElevatorSim(kV, kA, DCMotor.getNEO(1), 0, maxHeightMeters, true, 0);
+      new ElevatorSim(0, 0.01, DCMotor.getNEO(1), 0, maxHeightMeters, true, 0);
   private double volts = 0;
 
   @Override

src/main/java/org/curtinfrc/frc2025/util/LoggedTunableNumber.java

@@ -0,0 +1,123 @@
+// Copyright (c) 2025 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package org.curtinfrc.frc2025.util;
+
+import java.util.Arrays;
+import java.util.HashMap;
+import java.util.Map;
+import java.util.function.Consumer;
+import java.util.function.DoubleSupplier;
+import org.curtinfrc.frc2025.Constants;
+import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
+
+/**
+ * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or
+ * value not in dashboard.
+ */
+public class LoggedTunableNumber implements DoubleSupplier {
+  private static final String tableKey = "/Tuning";
+
+  private final String key;
+  private boolean hasDefault = false;
+  private double defaultValue;
+  private LoggedNetworkNumber dashboardNumber;
+  private Map<Integer, Double> lastHasChangedValues = new HashMap<>();
+
+  /**
+   * Create a new LoggedTunableNumber
+   *
+   * @param dashboardKey Key on dashboard
+   */
+  public LoggedTunableNumber(String dashboardKey) {
+    this.key = tableKey + "/" + dashboardKey;
+  }
+
+  /**
+   * Create a new LoggedTunableNumber with the default value
+   *
+   * @param dashboardKey Key on dashboard
+   * @param defaultValue Default value
+   */
+  public LoggedTunableNumber(String dashboardKey, double defaultValue) {
+    this(dashboardKey);
+    initDefault(defaultValue);
+  }
+
+  /**
+   * Set the default value of the number. The default value can only be set once.
+   *
+   * @param defaultValue The default value
+   */
+  public void initDefault(double defaultValue) {
+    if (!hasDefault) {
+      hasDefault = true;
+      this.defaultValue = defaultValue;
+      if (Constants.tuningMode) {
+        dashboardNumber = new LoggedNetworkNumber(key, defaultValue);
+      }
+    }
+  }
+
+  /**
+   * Get the current value, from dashboard if available and in tuning mode.
+   *
+   * @return The current value
+   */
+  public double get() {
+    if (!hasDefault) {
+      return 0.0;
+    } else {
+      return Constants.tuningMode ? dashboardNumber.get() : defaultValue;
+    }
+  }
+
+  /**
+   * Checks whether the number has changed since our last check
+   *
+   * @param id Unique identifier for the caller to avoid conflicts when shared between multiple
+   *     objects. Recommended approach is to pass the result of "hashCode()"
+   * @return True if the number has changed since the last time this method was called, false
+   *     otherwise.
+   */
+  public boolean hasChanged(int id) {
+    double currentValue = get();
+    Double lastValue = lastHasChangedValues.get(id);
+    if (lastValue == null || currentValue != lastValue) {
+      lastHasChangedValues.put(id, currentValue);
+      return true;
+    }
+
+    return false;
+  }
+
+  /**
+   * Runs action if any of the tunableNumbers have changed
+   *
+   * @param id Unique identifier for the caller to avoid conflicts when shared between multiple *
+   *     objects. Recommended approach is to pass the result of "hashCode()"
+   * @param action Callback to run when any of the tunable numbers have changed. Access tunable
+   *     numbers in order inputted in method
+   * @param tunableNumbers All tunable numbers to check
+   */
+  public static void ifChanged(
+      int id, Consumer<double[]> action, LoggedTunableNumber... tunableNumbers) {
+    if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) {
+      action.accept(Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray());
+    }
+  }
+
+  /** Runs action if any of the tunableNumbers have changed */
+  public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) {
+    ifChanged(id, values -> action.run(), tunableNumbers);
+  }
+
+  @Override
+  public double getAsDouble() {
+    return get();
+  }
+}