Add tunable elevator setpoints and PID with LoggedTunableNumber

Introduced the LoggedTunableNumber utility for runtime-tunable constants and integrated it into the Elevator subsystem for setpoints and PID gains. Updated ElevatorIO and ElevatorIOTalonFX to support dynamic PID and characterization parameter updates

Author: gitpaperclip

build.gradle

@@ -63,11 +63,6 @@ wpi.java.debugJni = false
 // Set this to true to enable desktop support.
 def includeDesktopSupport = true
 
-// Configuration for AdvantageKit
-task(replayWatch, type: JavaExec) {
-    mainClass = "org.littletonrobotics.junction.ReplayWatch"
-    classpath = sourceSets.main.runtimeClasspath
-}
 
 // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
 // Also defines JUnit 4.
@@ -215,4 +210,11 @@ spotless {
         indentWithSpaces(2)
         endWithNewline()
     }
+
+    // Configuration for AdvantageKit
+    task(replayWatch, type: JavaExec) {
+        mainClass = "org.littletonrobotics.junction.ReplayWatch"
+        classpath = sourceSets.main.runtimeClasspath
+    }
+
 }

src/main/java/frc/robot/Constants.java

@@ -34,4 +34,7 @@ public static enum Mode {
     /** Replaying from a log file. */
     REPLAY
   }
+
+  public static final boolean tuningMode = false;
+  public static final boolean disableHAL = false;
 }

src/main/java/frc/robot/subsystems/elevator/Elevator.java

@@ -1,5 +1,81 @@
 package frc.robot.subsystems.elevator;
 
-public class Elevator {
-    
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.Debouncer.DebounceType;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.util.LoggedTunableNumber;
+import java.util.function.BooleanSupplier;
+import java.util.function.Supplier;
+import org.littletonrobotics.junction.AutoLogOutput;
+
+public class Elevator extends SubsystemBase {
+
+  private static final LoggedTunableNumber L4Setpoint =
+      new LoggedTunableNumber("Elevator/Setpoints/L4");
+  private static final LoggedTunableNumber L3Setpoint =
+      new LoggedTunableNumber("Elevator/Setpoints/L3");
+  private static final LoggedTunableNumber L2Setpoint =
+      new LoggedTunableNumber("Elevator/Setpoints/L2");
+  private static final LoggedTunableNumber L1Setpoint =
+      new LoggedTunableNumber("Elevator/Setpoints/L1");
+  private static final LoggedTunableNumber ReefAlgaeTopSetpoint =
+      new LoggedTunableNumber("Elevator/Setpoints/ReefTop");
+  private static final LoggedTunableNumber ReefAlgaeBotSetpoint =
+      new LoggedTunableNumber("Elevator/Setpoints/ReefBot");
+  private static final LoggedTunableNumber GroundAlgaeSetpoint =
+      new LoggedTunableNumber("Elevator/Setpoints/Ground");
+  private static final LoggedTunableNumber ProcessorSetpoint =
+      new LoggedTunableNumber("Elevator/Setpoints/Processor");
+  private static final LoggedTunableNumber BalanceAlgaeSetpoint =
+      new LoggedTunableNumber("Elevator/Setpoints/Balanced");
+  private static final LoggedTunableNumber PreclimbSetpoint =
+      new LoggedTunableNumber("Elevator/Setpoints/Preclimb");
+
+  private static final LoggedTunableNumber kP = new LoggedTunableNumber("Elevator/PID/P");
+  private static final LoggedTunableNumber kI = new LoggedTunableNumber("Elevator/PID/I");
+  private static final LoggedTunableNumber kD = new LoggedTunableNumber("Elevator/PID/D");
+  private static final LoggedTunableNumber kA = new LoggedTunableNumber("Elevator/PID/A");
+  private static final LoggedTunableNumber kV = new LoggedTunableNumber("Elevator/PID/V");
+  private static final LoggedTunableNumber kS = new LoggedTunableNumber("Elevator/PID/S");
+  private static final LoggedTunableNumber kG = new LoggedTunableNumber("Elevator/PID/G");
+
+  private static final LoggedTunableNumber ClimbVoltage =
+      new LoggedTunableNumber("Elevator/Climbing/ClimbVoltage");
+  private static final LoggedTunableNumber ZeroVoltage =
+      new LoggedTunableNumber("Elevator/Climbing/ZeroVoltage");
+  private static final LoggedTunableNumber AutoZeroVoltage =
+      new LoggedTunableNumber("Elevator/Climbing/AutoZeroVoltage");
+
+  private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged();
+
+  private final Debouncer motorConnectedDebouncer = new Debouncer(0.5, DebounceType.kFalling);
+  private final Debouncer followerMotorConnectedDebouncer =
+      new Debouncer(0.5, DebounceType.kFalling);
+
+  private final Alert motorDisconnectedAlert =
+      new Alert("Elevator leader motor disconnected!", Alert.AlertType.kWarning);
+  private final Alert followerDisconnectedAlert =
+      new Alert("Elevator follower motor disconnected!", Alert.AlertType.kWarning);
+  private BooleanSupplier coastOverride = () -> false;
+  private BooleanSupplier disabledOverride = () -> false;
+
+  @AutoLogOutput private boolean brakeModeEnabled = true;
+
+  private final ElevatorIO io;
+
+  public Elevator(ElevatorIO io) {
+    this.io = io;
+  }
+
+  public void periodic() {
+    io.updateInputs(inputs);
+  }
+  //
+  //  private static final LoggedTunableNumber LimitDIO = new
+  // LoggedTunableNumber("Elevator/Limit/LimitValue");
+  //  private static final LoggedTunableNumber ServoPosition = new
+  // LoggedTunableNumber("Elevator/Servo/Position");
+
 }

src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java

@@ -3,58 +3,46 @@
 import org.littletonrobotics.junction.AutoLog;
 
 public interface ElevatorIO {
- @AutoLog
- class ElevatorIOInputs {
-   public ElevatorIOData data = new ElevatorIOData(
-    false,
-    false,
-    0.0,
-    0.0,
-    0.0,
-    0.0,
-    0.0,
-    0.0,
-    0.0,
-    0.0, 
-    0.0, 
-    0.0, 
-    0.0, 
-    0.0
-    );
- }
-    record ElevatorIOData (
-        boolean masterConnected,
-        boolean followerConnected,
-        double masterPositionRad,
-        double followerPositionRad,
-        double masterVelocityRadPerSec,
-        double followerVelocityRadPerSec,
-        double masterAppliedVolts,
-        double followerAppliedVolts,
-        double masterTorqueCurrentAmps,
-        double followerTorqueCurrentAmps,
-        double masterSupplyCurrentAmps,
-        double followerSupplyCurrentAmps,
-        double masterTemp,
-        double followerTemp
-    ) {}
-    
-    default void updateInputs(ElevatorIOInputs inputs) {}
-
-    default void runOpenLoop(double output) {}
-  
-    default void runVolts(double volts) {}
-
-    default void MotionMagic(double volts) {}
-  
-    default void stop() {}
-
-    default void runPosition(double positionRad) {}
-  
-    default void setPID(double kP, double kI, double kD) {}
-  
-    default void setBrakeMode(boolean enabled) {}
-
-    default void coast() {}
-
-} 
+  @AutoLog
+  class ElevatorIOInputs {
+    public ElevatorIOData data =
+        new ElevatorIOData(
+            false, false, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+  }
+
+  record ElevatorIOData(
+      boolean masterConnected,
+      boolean followerConnected,
+      double masterPositionRad,
+      double followerPositionRad,
+      double masterVelocityRadPerSec,
+      double followerVelocityRadPerSec,
+      double masterAppliedVolts,
+      double followerAppliedVolts,
+      double masterTorqueCurrentAmps,
+      double followerTorqueCurrentAmps,
+      double masterSupplyCurrentAmps,
+      double followerSupplyCurrentAmps,
+      double masterTemp,
+      double followerTemp) {}
+
+  default void updateInputs(ElevatorIOInputs inputs) {}
+
+  default void runOpenLoop(double output) {}
+
+  default void runVolts(double volts) {}
+
+  default void MotionMagic(double volts) {}
+
+  default void coast() {}
+
+  default void stop() {}
+
+  default void runPosition(double positionRad) {}
+
+  default void setPID(double kP, double kI, double kD) {}
+
+  default void setCharacterization(double kA, double kV, double kG, double kS) {}
+
+  default void setBrakeMode(boolean enabled) {}
+}