diff --git a/.github/workflows/checks.yml b/.github/workflows/checks.yml index b855262..12bde68 100644 --- a/.github/workflows/checks.yml +++ b/.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 diff --git a/.vscode/settings.json b/.vscode/settings.json index 50e3d8c..d9d3cab 100644 --- a/.vscode/settings.json +++ b/.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 } diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 0ce8617..9d51ee6 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/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 diff --git a/src/main/java/frc/robot/components/camera/Camera.java b/src/main/java/frc/robot/components/camera/Camera.java index 2ff9563..1907c59 100644 --- a/src/main/java/frc/robot/components/camera/Camera.java +++ b/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(); + } } diff --git a/src/main/java/frc/robot/components/camera/CameraIO.java b/src/main/java/frc/robot/components/camera/CameraIO.java index edf991c..0c2297d 100644 --- a/src/main/java/frc/robot/components/camera/CameraIO.java +++ b/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(); diff --git a/src/main/java/frc/robot/components/camera/CameraIOReal.java b/src/main/java/frc/robot/components/camera/CameraIOReal.java index 6bbf9ed..2de23db 100644 --- a/src/main/java/frc/robot/components/camera/CameraIOReal.java +++ b/src/main/java/frc/robot/components/camera/CameraIOReal.java @@ -46,4 +46,9 @@ public void setSimPose(Optional simEst, boolean newResult) { public CameraConstants getCameraConstants() { return constants; } + + @Override + public void close() throws Exception { + camera.close(); + } } diff --git a/src/main/java/frc/robot/components/camera/CameraIOSim.java b/src/main/java/frc/robot/components/camera/CameraIOSim.java index b6a40a6..6161c72 100644 --- a/src/main/java/frc/robot/components/camera/CameraIOSim.java +++ b/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(); + } } diff --git a/src/main/java/frc/robot/components/rollers/RollerIO.java b/src/main/java/frc/robot/components/rollers/RollerIO.java index 46cea56..4bea31f 100644 --- a/src/main/java/frc/robot/components/rollers/RollerIO.java +++ b/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(); + } } diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 6bd3118..f286754 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/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(); diff --git a/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java index 44cceff..a594a4f 100644 --- a/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java +++ b/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(); + } } diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index 4346925..32a16d5 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/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 + } } diff --git a/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java index b4dcfd1..d1c03b6 100644 --- a/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java +++ b/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(); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 9a6196f..3d18615 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/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(); diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index a400db1..3678cec 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/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 + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java index 304e65b..867b1cf 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java +++ b/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(); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java index 3c38920..8da29d9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java +++ b/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(); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index cacef67..a95cda3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/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 diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index cd4ab04..cf6de47 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/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(); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 685b95a..487b139 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/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 + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index e422313..7b4de26 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/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(); + } + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIO.java b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIO.java index 0a51c1e..e746e95 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIO.java +++ b/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 { diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOReal.java b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOReal.java index 8c4e16d..9a50eaf 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOReal.java +++ b/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(); + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOSim.java b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOSim.java index 512203d..7a6f8b4 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOSim.java +++ b/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 + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/module/Module.java b/src/main/java/frc/robot/subsystems/swerve/module/Module.java index cb550c4..c0637c4 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/Module.java +++ b/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(); + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOReal.java index cc29237..7f62fde 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOReal.java @@ -22,7 +22,7 @@ import org.littletonrobotics.junction.AutoLog; import org.littletonrobotics.junction.Logger; -public class ModuleIOReal { +public class ModuleIOReal implements AutoCloseable { @AutoLog public static class ModuleIOInputs { @@ -228,4 +228,11 @@ public void setTurnPositionSetpoint(Rotation2d setpoint) { public ModuleConstants getModuleConstants() { return constants; } + + @Override + public void close() throws Exception { + driveTalon.close(); + turnTalon.close(); + cancoder.close(); + } } diff --git a/src/test/java/frc/robot/SuperstructureTest.java b/src/test/java/frc/robot/SuperstructureTest.java new file mode 100644 index 0000000..c968979 --- /dev/null +++ b/src/test/java/frc/robot/SuperstructureTest.java @@ -0,0 +1,392 @@ +package frc.robot; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import com.ctre.phoenix6.CANBus; +import edu.wpi.first.hal.HAL; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Superstructure.SuperState; +import frc.robot.components.rollers.RollerIO; +import frc.robot.subsystems.indexer.Indexer; +import frc.robot.subsystems.indexer.LindexerSubsystem; +import frc.robot.subsystems.intake.FintakeSubsystem; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.intake.LintakeSubsystem; +import frc.robot.subsystems.shooter.FlywheelIO; +import frc.robot.subsystems.shooter.HoodIO; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.ShooterSubsystem; +import frc.robot.subsystems.swerve.SwerveSubsystem; +import java.lang.reflect.Field; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +public class SuperstructureTest { + Superstructure superstructure; + Intake intake; + Shooter shooter; + Indexer routing; + SwerveSubsystem swerve; + + boolean scoreReq; + boolean intakeReq; + boolean feedReq; + boolean flowReq; + boolean antiJamReq; + boolean isFull; + boolean isEmpty; + + @BeforeEach // Runs before each test + void setup() { + // TODO: SETUP + // Reset command scheduler + Field instance; + try { + instance = CommandScheduler.class.getDeclaredField("instance"); + instance.setAccessible(true); + instance.set(null, null); + } catch (Exception e) { + e.printStackTrace(); + } + + assert HAL.initialize(500, 0); // Initialize HAL, crash if failed + + DriverStationSim.setEnabled(true); + DriverStationSim.notifyNewData(); + + scoreReq = false; + intakeReq = false; + feedReq = false; + flowReq = false; + antiJamReq = false; + isFull = false; + isEmpty = true; + + intake = new FintakeSubsystem(new RollerIO(10, FintakeSubsystem.getIntakeConfig(), new CANBus())); + shooter = + new ShooterSubsystem( + new HoodIO(HoodIO.getHoodConfiguration(), new CANBus()), + new FlywheelIO(FlywheelIO.getFlywheelConfiguration(), new CANBus())); + routing = + new LindexerSubsystem( + new CANBus(), + new RollerIO(11, LindexerSubsystem.getIndexerConfigs(), new CANBus()), + new RollerIO(12, LindexerSubsystem.getKickerConfigs(), new CANBus())); + swerve = new SwerveSubsystem(new CANBus()); + + superstructure = + new Superstructure( + swerve, + routing, + intake, + shooter, + new Trigger(() -> scoreReq), + new Trigger(() -> intakeReq), + new Trigger(() -> feedReq), + new Trigger(() -> antiJamReq), + new Trigger(() -> isFull), + new Trigger(() -> isEmpty)); + } + + @AfterEach // Runs after each test + void tearDown() { + // TODO: TEARDOWN + try { + superstructure.close(); + } catch (Exception e) { + e.printStackTrace(); + } + } + + @Test + void idleToIntake() { + assertEquals( + SuperState.IDLE, Superstructure.getState()); // Verify that superstructure starts in IDLE + + // Some time passes... + for (int i = 0; i < 50; i++) { + CommandScheduler.getInstance().run(); + } + + assertEquals( + SuperState.IDLE, + Superstructure.getState()); // Verify that the superstructure hasn't transitioned yet + + intakeReq = true; // This should trigger the state transition from IDLE to INTAKE + + // Some time passes... + for (int i = 0; i < 50; i++) { + CommandScheduler.getInstance().run(); + } + + assertEquals( + SuperState.INTAKE, + Superstructure.getState()); // Verify that the superstructure has properly transitioned + + // TODO: THIS DOESN'T WORK BC THE AREN'T THE SAME COMMAND IN MEMORY. FIGURE OUT HOW TO FIX + // assertEquals(intake.getCurrentCommand(), intake.intake()); // Verify that the intake is + // intaking + } + + @Test + void intakeToReadyNotFull() { + idleToIntake(); // First, we need to get into intake + + isEmpty = false; // We're no longer empty + + // Some time passes... + for (int i = 0; i < 50; i++) { + CommandScheduler.getInstance().run(); + } + + assertEquals( + SuperState.INTAKE, + Superstructure.getState()); // Should still be intaking bc the request is not off + + intakeReq = false; + + // Some time passes... + for (int i = 0; i < 50; i++) { + CommandScheduler.getInstance().run(); + } + + assertEquals( + SuperState.READY, + Superstructure + .getState()); // Should be in READY because we're not empty and intakeReq is false + } + + @Test + void intakeToReadyFull() { + idleToIntake(); // Enter Intake + + isEmpty = false; // We have at least 1 ball + + // Some time passes... + for (int i = 0; i < 50; i++) { + CommandScheduler.getInstance().run(); + } + + assertEquals( + SuperState.INTAKE, + Superstructure + .getState()); // Should still be intaking because we're not full and the request is not + // off + + isFull = true; // Full + + // Some time passes... + for (int i = 0; i < 50; i++) { + CommandScheduler.getInstance().run(); + } + + assertEquals(SuperState.READY, Superstructure.getState()); // Should be ready because we're full + assertEquals(true, intakeReq); // Even though we're still requesting to intake + } + + @Test + void readyToSpinUpScore() { + intakeToReadyNotFull(); // Get into ready + + scoreReq = true; // I.e. press button to start scoring + + // + CommandScheduler.getInstance().run(); + + // Same note as readyToSpinUpFeed + assertEquals(SuperState.SPIN_UP_SCORE, Superstructure.getState()); + } + + @Test + void scoreToIdle() { + // readyToScore(); // Start scoring + + assertEquals(SuperState.SCORE, Superstructure.getState()); // Ensure we're still scoring + assertEquals(true, scoreReq); + + scoreReq = false; + + // Some time passes... + for (int i = 0; i < 50; i++) { + CommandScheduler.getInstance().run(); + } + + assertEquals( + SuperState.SCORE, + Superstructure.getState()); // Should still score because we only transition when empty + + isEmpty = true; // We've shot our whole hopper + + // Some time passes... + for (int i = 0; i < 50; i++) { + CommandScheduler.getInstance().run(); + } + + assertEquals(SuperState.IDLE, Superstructure.getState()); + } + + @Test + void readyToSpinUpFeed() { + intakeToReadyNotFull(); // Get into ready + + feedReq = true; // I.e. press button to start scoring + + // One cycle to change states + CommandScheduler.getInstance().run(); + // I believe this test is failing bc when the check runs, the flywheel actual and setpoint + // velocity are both zero. Fixed in bringup by adding a debounce. When that gets merged, will + // work on maiking this pass + assertEquals(SuperState.SPIN_UP_FEED, Superstructure.getState()); + } + + @Test + void feedToIdle() { + // readyToFeed(); // Start feeding + + assertEquals(SuperState.FEED, Superstructure.getState()); // Ensure we're still scoring + assertEquals(true, feedReq); + + feedReq = false; + + // Some time passes... + for (int i = 0; i < 50; i++) { + CommandScheduler.getInstance().run(); + } + + assertEquals( + SuperState.FEED, + Superstructure.getState()); // Should still score because we only transition when empty + + isEmpty = true; // We've shot our whole hopper + + // Some time passes... + for (int i = 0; i < 50; i++) { + CommandScheduler.getInstance().run(); + } + + assertEquals(SuperState.IDLE, Superstructure.getState()); + } + + @Test + void feedToFeedFlow() { + // readyToFeed(); // Get into feed + + assertEquals(SuperState.FEED, Superstructure.getState()); // Ensure we're still feeding + assertEquals(true, feedReq); // Make sure we're still requesting to feed + + flowReq = true; // Request to flow + + // Some time passes... + for (int i = 0; i < 50; i++) { + CommandScheduler.getInstance().run(); + } + + assertEquals(SuperState.FEED_FLOW, Superstructure.getState()); // Should be in FEED_FLOW now + } + + @Test + void scoreToScoreFlow() { + // readyToScore(); // Get into score + + assertEquals(SuperState.SCORE, Superstructure.getState()); // Ensure we're still feeding + assertEquals(true, scoreReq); // Make sure we're still requesting to feed + + flowReq = true; // Request to flow + + // Some time passes... + for (int i = 0; i < 50; i++) { + CommandScheduler.getInstance().run(); + } + + assertEquals(SuperState.SCORE_FLOW, Superstructure.getState()); // Should be in FEED_FLOW now + } + + @Test + void idleToFeedFlow() { + assertEquals(SuperState.IDLE, Superstructure.getState()); // Ensure we start in IDLE + assertEquals(feedReq, false); + + flowReq = true; // We want to flow + + // Some time passes... + for (int i = 0; i < 50; i++) { + CommandScheduler.getInstance().run(); + } + + assertEquals( + SuperState.IDLE, + Superstructure.getState()); // Shouldn't transition yet bc feedReq is still false + + feedReq = true; // We want to feed + + // Some time passes... + for (int i = 0; i < 50; i++) { + CommandScheduler.getInstance().run(); + } + + assertEquals(SuperState.FEED_FLOW, Superstructure.getState()); // Should be in FEED_FLOW now + } + + @Test + void idleToScoreFlow() { + assertEquals(SuperState.IDLE, Superstructure.getState()); // Ensure we start in IDLE + assertEquals(scoreReq, false); + + flowReq = true; // We want to flow + + // Some time passes... + for (int i = 0; i < 50; i++) { + CommandScheduler.getInstance().run(); + } + + assertEquals( + SuperState.IDLE, + Superstructure.getState()); // Shouldn't transition yet bc scoreReq is still false + + scoreReq = true; // We want to score + + // Some time passes... + for (int i = 0; i < 50; i++) { + CommandScheduler.getInstance().run(); + } + + assertEquals(SuperState.SCORE_FLOW, Superstructure.getState()); // Should be in SCORE_FLOW now + } + + @Test + void feedFlowToIdle() { + idleToFeedFlow(); // Get into feed flow + assertEquals(SuperState.FEED_FLOW, Superstructure.getState()); + assertEquals(true, flowReq); + assertEquals(true, feedReq); + + flowReq = false; + + // Some time passes... + for (int i = 0; i < 50; i++) { + CommandScheduler.getInstance().run(); + } + + assertEquals(SuperState.IDLE, Superstructure.getState()); + } + + @Test + void scoreFlowToIdle() { + idleToScoreFlow(); // Get into feed flow + assertEquals(SuperState.SCORE_FLOW, Superstructure.getState()); + assertEquals(true, flowReq); + assertEquals(true, scoreReq); + + flowReq = false; + + // Some time passes... + for (int i = 0; i < 50; i++) { + CommandScheduler.getInstance().run(); + } + + assertEquals(SuperState.IDLE, Superstructure.getState()); + } +}