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

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

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

@@ -11,6 +11,7 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.subsystems.indexer.Indexer;
+import frc.robot.subsystems.indexer.LindexerSubsystem;
 import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
@@ -19,7 +20,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 {
@@ -113,6 +114,35 @@ public Superstructure(
     stateTimer.start();
   }
 
+  // Used for testing
+  public Superstructure(
+      SwerveSubsystem swerve,
+      Indexer indexer,
+      Intake intake,
+      Shooter shooter,
+      Trigger scoreReq,
+      Trigger intakeReq,
+      Trigger feedReq,
+      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.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));
@@ -149,7 +179,7 @@ private void addTransitions() {
         (intakeReq.negate().and(scoreReq.negate()).and(isEmpty.negate())));
     // .or(isFull));
 
-    // bindTransition(SuperState.INTAKE, SuperState.SPIN_UP_FEED, feedReq);
+    bindTransition(SuperState.READY, SuperState.SPIN_UP_FEED, feedReq);
 
     bindTransition(SuperState.READY, SuperState.INTAKE, intakeReq);
     // .and(isFull.negate()));
@@ -351,6 +381,15 @@ 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;
+  }
+
   private Alliance getStartingAlliance() {
     String gameData = DriverStation.getGameSpecificMessage();
     // gives first inactive alliance

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/RollerIO.java

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

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

@@ -7,7 +7,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 
 /** Add your docs here. */
-public interface Indexer {
+public interface Indexer extends AutoCloseable {
 
   public boolean isFull();
 

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

@@ -17,8 +17,8 @@
 import frc.robot.components.rollers.RollerIOInputsAutoLogged;
 import org.littletonrobotics.junction.Logger;
 
-/** Lindexer = Linear Indexer. !! ALPHA !! */
 public class LindexerSubsystem extends SubsystemBase implements Indexer {
+  // Add actual CanBus
 
   public static final double GEAR_RATIO = 2.0;
   private CANrangeIOReal firstCANRangeIO;
@@ -175,4 +175,10 @@ public Command runRollerSysId() {
         indexRollerSysid.dynamic(Direction.kForward),
         indexRollerSysid.dynamic(Direction.kReverse));
   }
+
+  @Override
+  public void close() throws Exception {
+    indexRollerIO.close();
+    kickerIO.close();
+  }
 }

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

@@ -58,4 +58,9 @@ public Command rest() {
     // TODO Auto-generated method stub
     throw new UnsupportedOperationException("Unimplemented method 'rest'");
   }
+
+  @Override
+  public void close() throws Exception {
+      // No-op rn bc nothing to close
+  }
 }

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

@@ -81,4 +81,9 @@ public static TalonFXConfiguration getIntakeConfig() {
 
     return config;
   }
+
+  @Override
+  public void close() throws Exception {
+    io.close();
+  }
 }

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

@@ -7,7 +7,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 
 /** Add your docs here. */
-public interface Intake {
+public interface Intake extends AutoCloseable {
   /** Run balls towards the shooter */
   public Command intake();
 

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

@@ -34,4 +34,9 @@ public Command rest() {
     // TODO Auto-generated method stub
     throw new UnsupportedOperationException("Unimplemented method 'rest'");
   }
+
+  @Override
+  public void close() throws Exception {
+      // No-op rn bc nothing to close
+  }
 }

src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java

@@ -24,7 +24,7 @@
 import org.littletonrobotics.junction.AutoLogOutput;
 
 /** Add your docs here. */
-public class FlywheelIO {
+public class FlywheelIO implements AutoCloseable {
 
   @AutoLog
   public static class FlywheelIOInputs {
@@ -183,4 +183,10 @@ public void updateInputs(FlywheelIOInputs inputs) {
   public double getSetpointRotPerSec() {
     return velocitySetpointRotPerSec;
   }
+
+  @Override
+  public void close() {
+    flywheelFollower.close();
+    flywheelLeader.close();
+  }
 }

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

@@ -25,7 +25,7 @@
 import org.littletonrobotics.junction.AutoLog;
 import org.littletonrobotics.junction.AutoLogOutput;
 
-public class HoodIO {
+public class HoodIO implements AutoCloseable {
   /** Creates a new HoodIOReal. */
   @AutoLog
   public static class HoodIOInputs {
@@ -143,4 +143,9 @@ public Rotation2d getHoodSetpoint() {
   public void resetEncoder(Rotation2d rotations) {
     hoodMotor.setPosition(rotations.getRotations());
   }
+
+  @Override
+  public void close() throws Exception {
+    hoodMotor.close();
+  }
 }

src/main/java/frc/robot/subsystems/shooter/Shooter.java

@@ -9,7 +9,7 @@
 import java.util.function.Supplier;
 
 /** Add your docs here. */
-public interface Shooter {
+public interface Shooter extends AutoCloseable {
 
   /**
    * Sets hood angle and flywheel velocity based on distance from hub from the shot map + current

src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java

@@ -23,9 +23,8 @@
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
-/** Fixed shooter. !! ALPHA !! */
 public class ShooterSubsystem extends SubsystemBase implements Shooter {
-  public static double HOOD_GEAR_RATIO = 24.230769;
+  public static double HOOD_GEAR_RATIO = 147.0 / 13.0;
   public static Rotation2d HOOD_MAX_ROTATION = Rotation2d.fromDegrees(40);
   public static Rotation2d HOOD_MIN_ROTATION = Rotation2d.fromDegrees(2);
 
@@ -184,4 +183,10 @@ public boolean atHoodSetpoint() {
   public Command zeroHood() {
     return this.runOnce(() -> hoodIO.resetEncoder(HOOD_MIN_ROTATION));
   }
+
+  @Override
+  public void close() throws Exception {
+    flywheelIO.close();
+    hoodIO.close();
+  }
 }

src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java

@@ -66,4 +66,9 @@ public Command testShoot() {
     // TODO Auto-generated method stub
     throw new UnsupportedOperationException("Unimplemented method 'testShoot'");
   }
+
+  @Override
+  public void close() throws Exception {
+      // No-op rn bc nothing to close
+  }
 }

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

@@ -64,7 +64,7 @@
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
-public class SwerveSubsystem extends SubsystemBase {
+public class SwerveSubsystem extends SubsystemBase implements AutoCloseable {
   // decide which set of swerve constants to use based on robot edition
   // defaulting to comp is probably safer?
   public static final SwerveConstants SWERVE_CONSTANTS =
@@ -737,4 +737,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();
+  }
 }