Superstructure tests

.github/workflows/checks.yml

@@ -32,8 +32,9 @@ jobs:
       run: chmod +x gradlew
 
     # Runs a single command using the runners shell
-    - name: Compile and run tests on robot code
-      run: ./gradlew build
+    # Assemble builds the code without running tests
+    - name: Compile robot code
+      run: ./gradlew assemble
 
   format:
     name: Format
@@ -62,3 +63,31 @@ jobs:
     # Runs a single command using the runners shell
     - name: Run formatting check
       run: ./gradlew spotlessCheck
+
+  test:
+    name: Test
+
+    runs-on: ubuntu-latest
+
+    # Wpilib docker container
+    # Latest as of 1/11/26
+    # Need to use a container without -py. The ones with -py don't have java installed
+    container: wpilib/roborio-cross-ubuntu:2025-22.04
+
+    # Steps represent a sequence of tasks that will be executed as part of the job
+    # Copied from Onseason implementation
+    steps:
+    # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it
+    - uses: actions/checkout@v3
+
+    # Declares the repository safe and not under dubious ownership.
+    - name: Add repository to git safe directories
+      run: git config --global --add safe.directory $GITHUB_WORKSPACE
+
+    # Grant execute permission for gradlew
+    - name: Grant execute permission for gradlew
+      run: chmod +x gradlew
+
+    # Runs a single command using the runners shell
+    - name: Run tests
+      run: ./gradlew test

.vscode/settings.json

@@ -59,5 +59,9 @@
   ],
   "java.dependency.enableDependencyCheckup": false,
   "wpilib.selectDefaultSimulateExtension": false,
-  "wpilib.skipSelectSimulateExtension": true
+  "wpilib.skipSelectSimulateExtension": true,
+  "java.project.sourcePaths": [
+    "src/main",
+    "src/test"
+  ]
 }

src/main/java/frc/robot/Superstructure.java

@@ -18,7 +18,7 @@
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
-public class Superstructure {
+public class Superstructure implements AutoCloseable {
 
   /** We should have a state for every single action the robot will perform. */
   public enum SuperState {
@@ -104,6 +104,37 @@ public Superstructure(
     stateTimer.start();
   }
 
+  // Used for testing
+  public Superstructure(
+      SwerveSubsystem swerve,
+      IndexerSubsystem indexer,
+      IntakeSubsystem intake,
+      HoodSubsystem shooter,
+      Trigger scoreReq,
+      Trigger intakeReq,
+      Trigger feedReq,
+      Trigger flowReq,
+      Trigger antiJamReq,
+      Trigger isFull,
+      Trigger isEmpty) {
+    this.swerve = swerve;
+    this.intake = intake;
+    this.indexer = indexer;
+    this.shooter = shooter;
+    this.scoreReq = scoreReq;
+    this.intakeReq = intakeReq;
+    this.feedReq = feedReq;
+    this.flowReq = flowReq;
+    this.antiJamReq = antiJamReq;
+    this.isFull = isFull;
+    this.isEmpty = isEmpty;
+    this.driver = null;
+    this.operator = null;
+
+    addTransitions();
+    addCommands();
+  }
+
   private void addTriggers() {
     // Toggles for feeding
     operator.leftBumper().onTrue(Commands.runOnce(() -> shouldFeed = true));
@@ -118,7 +149,12 @@ private void addTriggers() {
 
     intakeReq = driver.leftTrigger().and(DriverStation::isTeleop).or(Autos.autoIntakeReq);
 
-    feedReq = driver.rightBumper().and(DriverStation::isTeleop).and(() -> shouldFeed == true).or(Autos.autoFeedReq);
+    feedReq =
+        driver
+            .rightBumper()
+            .and(DriverStation::isTeleop)
+            .and(() -> shouldFeed == true)
+            .or(Autos.autoFeedReq);
 
     flowReq = operator.rightTrigger();
 
@@ -159,10 +195,16 @@ private void addTransitions() {
 
       bindTransition(SuperState.FEED_FLOW, SuperState.FEED, flowReq.negate().and(feedReq));
 
-      bindTransition(SuperState.FEED_FLOW, SuperState.READY, flowReq.negate().and(feedReq.negate()).and(isEmpty.negate()));
+      bindTransition(
+          SuperState.FEED_FLOW,
+          SuperState.READY,
+          flowReq.negate().and(feedReq.negate()).and(isEmpty.negate()));
 
       // No so sure about the end condition here.
-      bindTransition(SuperState.FEED_FLOW, SuperState.IDLE, flowReq.negate().and(isEmpty).and(feedReq.negate()));
+      bindTransition(
+          SuperState.FEED_FLOW,
+          SuperState.IDLE,
+          flowReq.negate().and(isEmpty).and(feedReq.negate()));
     }
 
     // SCORE_FLOW transitions
@@ -173,10 +215,16 @@ private void addTransitions() {
 
       bindTransition(SuperState.SCORE_FLOW, SuperState.SCORE, flowReq.negate().and(scoreReq));
 
-      bindTransition(SuperState.SCORE_FLOW, SuperState.READY, flowReq.negate().and(scoreReq.negate()).and(isEmpty.negate()));
+      bindTransition(
+          SuperState.SCORE_FLOW,
+          SuperState.READY,
+          flowReq.negate().and(scoreReq.negate()).and(isEmpty.negate()));
 
       // No so sure about the end condition here.
-      bindTransition(SuperState.SCORE_FLOW, SuperState.IDLE, flowReq.negate().and(isEmpty).and(scoreReq.negate()));
+      bindTransition(
+          SuperState.SCORE_FLOW,
+          SuperState.IDLE,
+          flowReq.negate().and(isEmpty).and(scoreReq.negate()));
     }
 
     // Transition from any state to SPIT for anti jamming
@@ -308,4 +356,13 @@ public static SuperState getState() {
   public boolean stateIsIdle() {
     return getState() == SuperState.IDLE;
   }
+
+  @Override
+  public void close() throws Exception {
+    intake.close();
+    shooter.close();
+    indexer.close();
+    swerve.close();
+    Superstructure.state = SuperState.IDLE;
+  }
 }

src/main/java/frc/robot/components/camera/Camera.java

@@ -30,7 +30,7 @@
 import org.photonvision.targeting.PhotonTrackedTarget;
 
 /** Add your docs here. */
-public class Camera {
+public class Camera implements AutoCloseable {
 
   // The intrinsics and distortion coefficients are only actually used for sim
   public record CameraConstants(
@@ -217,4 +217,9 @@ public CameraConstants getCameraConstants() {
   public Pose3d getPose() {
     return pose;
   }
+
+  @Override
+  public void close() throws Exception {
+    io.close();
+  }
 }

src/main/java/frc/robot/components/camera/CameraIO.java

@@ -11,7 +11,7 @@
 import org.photonvision.targeting.PhotonPipelineResult;
 
 /** Add your docs here. */
-public interface CameraIO {
+public interface CameraIO extends AutoCloseable {
   @AutoLog
   public static class CameraIOInputs {
     public PhotonPipelineResult result = new PhotonPipelineResult();

src/main/java/frc/robot/components/camera/CameraIOReal.java

@@ -46,4 +46,9 @@ public void setSimPose(Optional<EstimatedRobotPose> simEst, boolean newResult) {
   public CameraConstants getCameraConstants() {
     return constants;
   }
+
+  @Override
+  public void close() throws Exception {
+    camera.close();
+  }
 }

src/main/java/frc/robot/components/camera/CameraIOSim.java

@@ -88,4 +88,10 @@ public String getName() {
   public CameraConstants getCameraConstants() {
     return constants;
   }
+
+  @Override
+  public void close() throws Exception {
+    camera.close();
+    simCamera.close();
+  }
 }

src/main/java/frc/robot/components/rollers/RollerIOReal.java

@@ -13,7 +13,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import org.littletonrobotics.junction.AutoLog;
 
-public class RollerIOReal {
+public class RollerIOReal implements AutoCloseable {
 
   @AutoLog
   public static class RollerIOInputs {
@@ -84,4 +84,9 @@ public Command getVoltage() {
     // TODO Auto-generated method stub
     throw new UnsupportedOperationException("Unimplemented method 'getVoltage'");
   }
+
+  @Override
+  public void close() throws Exception {
+    rollerMotor.close();
+  }
 }

src/main/java/frc/robot/subsystems/hood/HoodIO.java

@@ -21,7 +21,7 @@
 import edu.wpi.first.units.measure.Voltage;
 import org.littletonrobotics.junction.AutoLog;
 
-public class HoodIO {
+public class HoodIO implements AutoCloseable {
   /** Creates a new HoodIOReal. */
   @AutoLog
   public static class HoodIOInputs {
@@ -123,4 +123,9 @@ public void updateInputs(HoodIOInputsAutoLogged inputs) {
     inputs.hoodSupplyCurrentAmp = hoodSupplyCurrent.getValueAsDouble();
     inputs.hoodTempC = hoodTemp.getValueAsDouble();
   }
+
+  @Override
+  public void close() throws Exception {
+    hoodMotor.close();
+  }
 }

src/main/java/frc/robot/subsystems/hood/HoodSubsystem.java

@@ -13,7 +13,7 @@
 import java.util.function.Supplier;
 import org.littletonrobotics.junction.Logger;
 
-public class HoodSubsystem extends SubsystemBase {
+public class HoodSubsystem extends SubsystemBase implements AutoCloseable {
   private HoodIO hoodIO;
   private HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged();
 
@@ -70,4 +70,9 @@ public Command spit() {
     return this.run(
         () -> hoodIO.setHoodPosition(Rotation2d.kZero)); // TODO: FLYWHEEL AND TUNE HOOD POS
   }
+
+  @Override
+  public void close() throws Exception {
+    hoodIO.close();
+  }
 }

src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java

@@ -12,7 +12,7 @@
 import frc.robot.components.rollers.RollerIOReal;
 import org.littletonrobotics.junction.Logger;
 
-public class IndexerSubsystem extends SubsystemBase {
+public class IndexerSubsystem extends SubsystemBase implements AutoCloseable {
   public static double GEAR_RATIO = 2.0;
   // Add actual CanBus
 
@@ -112,4 +112,9 @@ public void periodic() {
     rollerIO.updateInputs(rollerInputs);
     Logger.processInputs("Indexer/Roller", rollerInputs);
   }
+
+  @Override
+  public void close() throws Exception {
+    rollerIO.close();
+  }
 }

src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java

@@ -9,7 +9,7 @@
 import frc.robot.components.rollers.RollerIOReal;
 import org.littletonrobotics.junction.Logger;
 
-public class IntakeSubsystem extends SubsystemBase {
+public class IntakeSubsystem extends SubsystemBase implements AutoCloseable {
   public static double GEAR_RATIO = 2.0;
 
   private RollerIOReal io;
@@ -65,4 +65,9 @@ public static TalonFXConfiguration getIntakeIOConfig() {
 
     return config;
   }
+
+  @Override
+  public void close() throws Exception {
+    io.close();
+  }
 }

src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java

@@ -62,7 +62,7 @@
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
-public class SwerveSubsystem extends SubsystemBase {
+public class SwerveSubsystem extends SubsystemBase implements AutoCloseable {
   public static final SwerveConstants SWERVE_CONSTANTS = new AlphaSwerveConstants();
 
   private final Module[] modules; // Front Left, Front Right, Back Left, Back Right
@@ -725,4 +725,15 @@ public void simulationPeriodic() {
     // Log simulated pose
     Logger.recordOutput("MapleSim/Pose", swerveSimulation.getSimulatedDriveTrainPose());
   }
+
+  @Override
+  public void close() throws Exception {
+    for (Module module : modules) {
+      module.close();
+    }
+    gyroIO.close();
+    for (Camera camera : cameras) {
+      camera.close();
+    }
+  }
 }

src/main/java/frc/robot/subsystems/swerve/gyro/GyroIO.java

@@ -3,7 +3,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import org.littletonrobotics.junction.AutoLog;
 
-public interface GyroIO {
+public interface GyroIO extends AutoCloseable {
 
   @AutoLog
   public static class GyroIOInputs {

src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOReal.java

@@ -58,4 +58,9 @@ public void updateInputs(GyroIOInputs inputs) {
   public void setYaw(Rotation2d yaw) {
     pigeon.setYaw(yaw.getDegrees());
   }
+
+  @Override
+  public void close() throws Exception {
+    pigeon.close();
+  }
 }

src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOSim.java

@@ -23,4 +23,9 @@ public void updateInputs(GyroIOInputs inputs) {
     inputs.yaw = simulation.getGyroReading();
     inputs.yawVelocityRadPerSec = simulation.getMeasuredAngularVelocity().in(RadiansPerSecond);
   }
+
+  @Override
+  public void close() throws Exception {
+    // Nothing to close
+  }
 }

src/main/java/frc/robot/subsystems/swerve/module/Module.java

@@ -10,7 +10,7 @@
 import org.littletonrobotics.junction.Logger;
 
 // A single module
-public class Module {
+public class Module implements AutoCloseable {
   // Represents module specific constants
   public record ModuleConstants(
       int id, String prefix, int driveID, int turnID, int cancoderID, Rotation2d cancoderOffset) {}
@@ -144,4 +144,9 @@ public SwerveModuleState getState() {
   public void setTurnSetpoint(Rotation2d rotation) {
     io.setTurnPositionSetpoint(rotation);
   }
+
+  @Override
+  public void close() throws Exception {
+    io.close();
+  }
 }