Gains tuning utility class

src/main/java/frc/robot/subsystems/ArmPivot.java

@@ -4,6 +4,7 @@
 import static edu.wpi.first.units.Units.Seconds;
 import static edu.wpi.first.units.Units.Volts;
 
+import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.configs.TalonFXConfigurator;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
@@ -28,6 +29,7 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
 import frc.robot.Hardware;
+import frc.robot.util.GainsTuningEntry;
 import java.util.function.Supplier;
 
 public class ArmPivot extends SubsystemBase {
@@ -75,6 +77,19 @@ public class ArmPivot extends SubsystemBase {
   // TalonFX
   private final TalonFX motor;
 
+  // Testing tuning entry :P
+  private final boolean TUNING_ENABLED = false;
+  private final GainsTuningEntry PIDtuner =
+      new GainsTuningEntry(
+          "ArmPivot",
+          ARMPIVOT_KP,
+          ARMPIVOT_KI,
+          ARMPIVOT_KD,
+          ARMPIVOT_KA,
+          ARMPIVOT_KV,
+          ARMPIVOT_KS,
+          ARMPIVOT_KG);
+
   // alerts if motor is not connected.
   private final Alert NotConnectedError =
       new Alert("ArmPivot", "Motor not connected", AlertType.kError);
@@ -227,6 +242,17 @@ public void factoryDefaults() {
   public void periodic() {
     // Error that ensures the motor is connected
     NotConnectedError.set(notConnectedDebouncer.calculate(!motor.getMotorVoltage().hasUpdated()));
+    if (TUNING_ENABLED) {
+      Slot0Configs pidTunerConfigs = new Slot0Configs();
+      pidTunerConfigs.kP = PIDtuner.getP();
+      pidTunerConfigs.kI = PIDtuner.getI();
+      pidTunerConfigs.kD = PIDtuner.getD();
+      pidTunerConfigs.kS = PIDtuner.getS();
+      pidTunerConfigs.kV = PIDtuner.getV();
+      pidTunerConfigs.kA = PIDtuner.getA();
+      pidTunerConfigs.kG = PIDtuner.getG();
+      motor.getConfigurator().apply(pidTunerConfigs);
+    }
   }
 }
 //  -Samuel "Big North" Mallick

src/main/java/frc/robot/util/GainsTuningEntry.java

@@ -0,0 +1,122 @@
+package frc.robot.util;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+public class GainsTuningEntry {
+
+  private final NetworkTable table;
+  private String kSubsystemName;
+  private NetworkTableEntry kPEntry;
+  private NetworkTableEntry kIEntry;
+  private NetworkTableEntry kDEntry;
+  private NetworkTableEntry kAEntry;
+  private NetworkTableEntry kVEntry;
+  private NetworkTableEntry kSEntry;
+  private NetworkTableEntry kGEntry;
+  public boolean updated = true;
+
+  /* Base PID Feedback entires. Excludes kA, kV, kS, and kG
+   *
+   */
+  public GainsTuningEntry(String subsystemName, double kp, double ki, double kd) {
+    table = NetworkTableInstance.getDefault().getTable("Gains/" + subsystemName);
+    this.kSubsystemName = subsystemName;
+    kPEntry = table.getEntry("kP");
+    kIEntry = table.getEntry("kI");
+    kDEntry = table.getEntry("kD");
+    kAEntry = null;
+    kVEntry = null;
+    kSEntry = null;
+    kGEntry = null;
+    kPEntry.setDouble(kp);
+    kIEntry.setDouble(ki);
+    kDEntry.setDouble(kd);
+  }
+
+  /* With ALL Feedback + Feedforward entries, INCLUDING kA
+   *
+   */
+  public GainsTuningEntry(
+      String subsystemName,
+      double kp,
+      double ki,
+      double kd,
+      double ka,
+      double kv,
+      double ks,
+      double kg) {
+    this(subsystemName, kp, ki, kd);
+    kAEntry = table.getEntry("kA");
+    kVEntry = table.getEntry("kV");
+    kSEntry = table.getEntry("kS");
+    kGEntry = table.getEntry("kG");
+    kAEntry.setDouble(ka);
+    kVEntry.setDouble(kv);
+    kSEntry.setDouble(ks);
+    kGEntry.setDouble(kg);
+  }
+
+  /* With Feedback + Feedforward entries, except kA
+   *
+   */
+  public GainsTuningEntry(
+      String subsystemName, double kp, double ki, double kd, double kv, double ks, double kg) {
+    this(subsystemName, kp, ki, kd);
+    kVEntry = table.getEntry("kV");
+    kSEntry = table.getEntry("kS");
+    kGEntry = table.getEntry("kG");
+    kVEntry.setDouble(kv);
+    kSEntry.setDouble(ks);
+    kGEntry.setDouble(kg);
+  }
+
+  public String getSubsystem() {
+    return kSubsystemName;
+  }
+
+  public double getP() {
+    return kPEntry.getDouble(0);
+  }
+
+  public double getI() {
+    return kIEntry.getDouble(0);
+  }
+
+  public double getD() {
+    return kDEntry.getDouble(0);
+  }
+
+  public double getA() {
+    if (kAEntry == null) {
+      return 0;
+    } else {
+      return kAEntry.getDouble(0);
+    }
+  }
+
+  public double getV() {
+    if (kVEntry == null) {
+      return 0;
+    } else {
+      return kVEntry.getDouble(0);
+    }
+  }
+
+  public double getS() {
+    if (kSEntry == null) {
+      return 0;
+    } else {
+      return kSEntry.getDouble(0);
+    }
+  }
+
+  public double getG() {
+    if (kGEntry == null) {
+      return 0;
+    } else {
+      return kGEntry.getDouble(0);
+    }
+  }
+}