Feat/subsystems cleanup

build.gradle

@@ -35,18 +35,27 @@ deploy {
                     jvmArgs.add("-XX:+UseSerialGC")
                     jvmArgs.add("-XX:MaxGCPauseMillis=50")
 
-                    // Enable VisualVM connection
-                    jvmArgs.add("-Dcom.sun.management.jmxremote=true")
-                    jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198")
-                    jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false")
-                    jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false")
-                    jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false")
-                    jvmArgs.add("-Djava.rmi.server.hostname=10.64.23.2")
+                    // TO ENTER PROFILING MODE:
+                    // Run ./gradlew deploy -PprofileMode
+                    if (frc.project.hasProperty("profileMode")) {
+                        project.logger.lifecycle("\u001B[31m~~~~ PROFILING MODE ENABLED ~~~~\u001B[0m")
+                        project.logger.lifecycle("Connect JMX client to roborio-6423-frc-local:1099 or 10.64.23.2:1099 for profiling")
+                        // Enable VisualVM connection
+                        jvmArgs.add("-Dcom.sun.management.jmxremote=true")
+                        jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198")
+                        jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false")
+                        jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false")
+                        jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false")
+                        jvmArgs.add("-Djava.rmi.server.hostname=10.64.23.2")
+                    }
 
                     // RIO 2.0 ONLY
                     // final MAX_JAVA_HEAP_SIZE_MB = 100;
                     // jvmArgs.add("-Xmx" + MAX_JAVA_HEAP_SIZE_MB + "M")
                     // jvmArgs.add("-Xms" + MAX_JAVA_HEAP_SIZE_MB + "M")
+                    // jvmArgs.add("-XX:GCTimeRatio=5")
+                    // jvmArgs.add("-XX:+UseSerialGC")
+                    // jvmArgs.add("-XX:MaxGCPauseMillis=50")
                     // jvmArgs.add("-XX:+AlwaysPreTouch")
                 }
 
@@ -136,9 +145,6 @@ tasks.withType(JavaCompile) {
 
 // Configure pre-commit hooks on first build
 task installGitHooks(type: Copy) {
-    logger.info("~~~~~~~~~~~~~~~~~~~~~~~~~~~~")
-    logger.info("~~~ Installing Git Hooks ~~~")
-    logger.info("~~~~~~~~~~~~~~~~~~~~~~~~~~~~")
     from '.scripts'
     into '.git/hooks'
     logger.info("Successfully Installed Hooks!")

src/main/java/org/frc6423/lib/subsystems/Roller.java

@@ -9,7 +9,6 @@
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import java.util.function.DoubleSupplier;
 
@@ -94,14 +93,7 @@ protected Command runSpeed(double speedRpm) {
    * @return {@link Command}
    */
   protected Command holdSpeed() {
-    return Commands.sequence(
-        this.run(
-                () -> {
-                  var currentSpeed = hardware.getSpeedRpm();
-                  hardware.setSpeed(currentSpeed);
-                })
-            .until(() -> true),
-        this.run(() -> {}));
+    return runSpeed(() -> hardware.getSpeedRpm());
   }
 
   @Override

src/main/java/org/frc6423/lib/utilities/CtreUtils.java

@@ -0,0 +1,35 @@
+// Copyright (c) 2025 FRC 6423 - Ward Melville Iron Patriots
+// https://github.com/wmironpatriots
+// 
+// Open Source Software; you can modify and/or share it under the terms of
+// MIT license file in the root directory of this project
+
+package org.frc6423.lib.utilities;
+
+import com.ctre.phoenix6.StatusCode;
+import edu.wpi.first.wpilibj.DriverStation;
+import java.util.function.Supplier;
+
+/** Utilities for CTRe devices */
+public class CtreUtils {
+  /**
+   * Try to run {@link StatusCode} supplier until it returns a {@link StatusCode} of OK
+   *
+   * @param function {@link StatusCode} supplier
+   * @param maxRetries maximum amount of retries before giving up
+   * @param deviceId Id of device being controlled
+   * @return {@link StatusCode} representing error code
+   */
+  public static StatusCode tryUntilOk(Supplier<StatusCode> function, int maxRetries, int deviceId) {
+    StatusCode statusCode = StatusCode.OK;
+    for (int i = 0; i == maxRetries; i++) {
+      statusCode = function.get();
+      if (statusCode == StatusCode.OK) break;
+    }
+    if (statusCode != StatusCode.OK) {
+      DriverStation.reportError("ERROR: Device ID " + deviceId + " could not be configured", true);
+    }
+
+    return statusCode;
+  }
+}

src/main/java/org/frc6423/lib/utilities/NtUtils.java

@@ -0,0 +1,72 @@
+// Copyright (c) 2025 FRC 6423 - Ward Melville Iron Patriots
+// https://github.com/wmironpatriots
+// 
+// Open Source Software; you can modify and/or share it under the terms of
+// MIT license file in the root directory of this project
+
+package org.frc6423.lib.utilities;
+
+import edu.wpi.first.networktables.BooleanEntry;
+import edu.wpi.first.networktables.DoubleArrayEntry;
+import edu.wpi.first.networktables.DoubleEntry;
+import edu.wpi.first.networktables.IntegerEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+/** Utilities for {@link NetworkTables} */
+public class NtUtils {
+  /**
+   * Create a new {@link IntegerEntry}
+   *
+   * @param path {@link String} representing topic path
+   * @param defaultValue default entry value
+   * @return {@link DoubleEntry}
+   */
+  public static IntegerEntry createIntegerEntry(String path, int defaultValue) {
+    var entry = NetworkTableInstance.getDefault().getIntegerTopic(path).getEntry(defaultValue);
+    entry.set(defaultValue);
+
+    return entry;
+  }
+
+  /**
+   * Create a new {@link DoubleEntry}
+   *
+   * @param path {@link String} representing topic path
+   * @param defaultValue default entry value
+   * @return {@link DoubleEntry}
+   */
+  public static DoubleEntry createDoubleEntry(String path, double defaultValue) {
+    var entry = NetworkTableInstance.getDefault().getDoubleTopic(path).getEntry(defaultValue);
+    entry.set(defaultValue);
+
+    return entry;
+  }
+
+  /**
+   * Create a new {@link BooleanEntry}
+   *
+   * @param path {@link String} representing topic path
+   * @param defaultValue default entry value
+   * @return {@link BooleanEntry}
+   */
+  public static BooleanEntry createBooleanEntry(String path, boolean defaultValue) {
+    var entry = NetworkTableInstance.getDefault().getBooleanTopic(path).getEntry(defaultValue);
+    entry.set(defaultValue);
+
+    return entry;
+  }
+
+  /**
+   * Create a new {@link DoubleArrayEntry}
+   *
+   * @param path {@link String} representing topic path
+   * @param defaultValue default entry value
+   * @return {@link DoubleArrayEntry}
+   */
+  public static DoubleArrayEntry createDoubleArrayEntry(String path, double[] defaultValue) {
+    var entry = NetworkTableInstance.getDefault().getDoubleArrayTopic(path).getEntry(defaultValue);
+    entry.set(defaultValue);
+
+    return entry;
+  }
+}

src/main/java/org/frc6423/robot/Constants.java

@@ -18,13 +18,16 @@
  * <p>When creating a new constant, make sure it is static and final
  */
 public class Constants {
-  /** Constants that affect robot behavior during runtime */
+  /** Constants that affect robot bhavior during runtime */
   public class Flags {
-    /** Represents how often periodic robot logic will run */
+    /** Represents how often periodic robot logic will run 0.02 Seconds by default */
     public static final Time PERIOD = Seconds.of(0.02);
 
     /** When true, the robot will enable debug menus and log debug information */
     @Deprecated public static final boolean debugMode = false;
+
+    /** When true, tunable entries will be added to NT for subsystems Should be false by default */
+    public static final boolean TUNE_MODE = false;
   }
 
   /** Constants representing port IDs that devices are connected to */

src/main/java/org/frc6423/robot/subsystems/superstructure/Chute.java

@@ -0,0 +1,101 @@
+// Copyright (c) 2025 FRC 6423 - Ward Melville Iron Patriots
+// https://github.com/wmironpatriots
+// 
+// Open Source Software; you can modify and/or share it under the terms of
+// MIT license file in the root directory of this project
+
+package org.frc6423.robot.subsystems.superstructure;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import java.util.function.DoubleSupplier;
+import org.frc6423.lib.subsystems.Roller;
+import org.frc6423.lib.subsystems.RollerIO;
+import org.frc6423.lib.subsystems.RollerIONeo;
+import org.frc6423.lib.subsystems.RollerIONone;
+import org.frc6423.robot.Robot;
+
+/** {@link Roller} subsystem representing the rollers on the chute */
+public class Chute extends Roller {
+  /** CONSTANTS */
+  public static final double MINIMUM_STALL_CURRENT_AMPS = 30.0;
+
+  /**
+   * @return fake {@link Chute} subsystem
+   */
+  public static Chute none() {
+    return new Chute(new RollerIONone());
+  }
+
+  /**
+   * Factory for creating a {@link Arm} subsystem
+   *
+   * @return {@link Arm} Subsystem
+   */
+  public static Chute create() {
+    if (Robot.isReal()) {
+      return new Chute(new RollerIONeo());
+    } else {
+      return new Chute(new RollerIONone());
+    }
+  }
+
+  private Chute(RollerIO hardware) {
+    super("ArmRoller", hardware);
+  }
+
+  /**
+   * @return true if roller motor is stalling
+   */
+  public boolean isStalling() {
+    return super.isStalling(MINIMUM_STALL_CURRENT_AMPS);
+  }
+
+  /**
+   * Run roller at specified volts
+   *
+   * @param volts desired volts
+   * @return {@link Command}
+   */
+  public Command runVolts(DoubleSupplier volts) {
+    return super.runVolts(volts);
+  }
+
+  /**
+   * Run roller at specified volts
+   *
+   * @param volts desired volts
+   * @return {@link Command}
+   */
+  public Command runVolts(double volts) {
+    return super.runVolts(volts);
+  }
+
+  /**
+   * Run roller at specified speed
+   *
+   * @param speedRpm desired speed in revs per minute
+   * @return {@link Command}
+   */
+  public Command runSpeed(DoubleSupplier speedRpm) {
+    return super.runSpeed(speedRpm);
+  }
+
+  /**
+   * Run roller at specified speed
+   *
+   * @param speedRpm desired speed in revs per minute
+   * @return {@link Command}
+   */
+  public Command runSpeed(double speedRpm) {
+    return super.runSpeed(speedRpm);
+  }
+
+  /**
+   * Hold roller at current speed
+   *
+   * @return {@link Command}
+   */
+  public Command holdSpeed() {
+    return super.holdSpeed();
+  }
+}

src/main/java/org/frc6423/robot/subsystems/superstructure/Visualizer.java

@@ -7,7 +7,7 @@
 package org.frc6423.robot.subsystems.superstructure;
 
 import static edu.wpi.first.units.Units.Centimeters;
-import static org.frc6423.robot.subsystems.superstructure.arm.ArmPivot.*;
+import static org.frc6423.robot.subsystems.superstructure.arm.Arm.*;
 import static org.frc6423.robot.subsystems.superstructure.elevator.Elevator.MAX_EXTENSION_HEIGHT;
 
 import edu.wpi.first.math.geometry.Rotation2d;