Miscellaneous Changes

build.gradle

@@ -138,7 +138,7 @@ gversion {
     classPackage = "com.team2813"
     className    = "BuildConstants"
     dateFormat   = "yyyy-MM-dd HH:mm:ss z"
-    timeZone     = "America/New_York"
+    timeZone     = "America/Los_Angeles"
     indent       = "  "
 }
 

src/main/java/com/team2813/Robot.java

@@ -112,18 +112,19 @@ public void autonomousInit() {
   @Override
   public void autonomousPeriodic() {}
 
-  /** This function is called once when teleop is enabled. */
   @Override
-  public void teleopInit() {
-    // This makes sure that the autonomous stops running when
-    // teleop starts running. If you want the autonomous to
-    // continue until interrupted by another command, remove
-    // this line or comment it out.
+  public void autonomousExit() {
+    // This makes sure that the autonomous command isn't running after we exit the autonomous
+    // period
     if (autonomousCommand != null) {
       autonomousCommand.cancel();
     }
   }
 
+  /** This function is called once when teleop is enabled. */
+  @Override
+  public void teleopInit() {}
+
   /** This function is called periodically during operator control. */
   @Override
   public void teleopPeriodic() {}

src/main/java/com/team2813/commands/DriveCommands.java

@@ -83,9 +83,16 @@ public static Command joystickDrive(
                   linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
                   linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
                   omega * drive.getMaxAngularSpeedRadPerSec());
+
+          // We have to call DriverStation#getAlliance() only once, since the return value could
+          // change in between calls to it. So, we turn the Optional<Alliance> from
+          // DriverStation#getAlliance() to an Optional<Boolean> by mapping a Red alliance to
+          // `true`, and defaulting to `false` if the DriverStation#getAlliance() returned an empty
+          // optional. This achieves the intended behavior of having `true` if, and only if
+          // DriverStation#getAlliance() returned a non-empty option with Alliance.Red, but avoiding
+          // accidentally unwrapping an empty optional
           boolean isFlipped =
-              DriverStation.getAlliance().isPresent()
-                  && DriverStation.getAlliance().get() == Alliance.Red;
+              DriverStation.getAlliance().map((alliance) -> alliance == Alliance.Red).orElse(false);
           drive.runVelocity(
               ChassisSpeeds.fromFieldRelativeSpeeds(
                   speeds,

src/main/java/com/team2813/subsystems/drive/Drive.java

@@ -151,13 +151,16 @@ public Drive(
 
   @Override
   public void periodic() {
-    odometryLock.lock(); // Prevents odometry updates while reading data
-    gyroIO.updateInputs(gyroInputs);
-    Logger.processInputs("Drive/Gyro", gyroInputs);
-    for (var module : modules) {
-      module.periodic();
+    try {
+      odometryLock.lock(); // Prevents odometry updates while reading data
+      gyroIO.updateInputs(gyroInputs);
+      Logger.processInputs("Drive/Gyro", gyroInputs);
+      for (var module : modules) {
+        module.periodic();
+      }
+    } finally {
+      odometryLock.unlock();
     }
-    odometryLock.unlock();
 
     // Stop moving when disabled
     if (DriverStation.isDisabled()) {

src/main/java/com/team2813/subsystems/hopper/Hopper.java

@@ -38,14 +38,14 @@ public void stop() {
   }
 
   public Command intakeCommand() {
-    return new InstantCommand(() -> intake());
+    return new InstantCommand(this::intake, this);
   }
 
   public Command outtakeCommand() {
-    return new InstantCommand(() -> outtake());
+    return new InstantCommand(this::outtake, this);
   }
 
   public Command stopCommand() {
-    return new InstantCommand(() -> stop());
+    return new InstantCommand(this::stop, this);
   }
 }

src/test/java/com/team2813/CommandTester.java

@@ -0,0 +1,33 @@
+/*
+Copyright 2025-2026 Prospect Robotics SWENext Club
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+// This is taken from [lib2813](https://github.com/Prospect-Robotics/lib2813/blob/main/testing/src/main/java/com/team2813/lib2813/testing/junit/jupiter/CommandTester.java).
+// When lib2813 is on maven central, this file can be deleted in favor of adding a dependency on lib2813.
+package com.team2813;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+/**
+ * Allows tests to run commands.
+ *
+ * <p>Tests can get an instance by using {@link WPILibExtension}.
+ *
+ * @since 2.0.0
+ */
+public interface CommandTester {
+
+  /** Schedules the provided command and runs it until it completes. */
+  void runUntilComplete(Command command);
+}

src/test/java/com/team2813/RobotContainerTest.java

@@ -0,0 +1,19 @@
+package com.team2813;
+
+import edu.wpi.first.wpilibj2.command.Commands;
+import org.junit.jupiter.api.Assumptions;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.extension.ExtendWith;
+
+@ExtendWith(WPILibExtension.class)
+public class RobotContainerTest {
+  @Test
+  public void canCreateRobotAndRunPeriodic(CommandTester tester) {
+    Assumptions.assumeTrue(
+        Constants.simMode == Constants.Mode.SIM, "The sim mode must be sim to run tests!");
+    // create a robot container
+    RobotContainer robotContainer = new RobotContainer();
+    // Run one periodic cycle
+    tester.runUntilComplete(Commands.none());
+  }
+}

src/test/java/com/team2813/WPILibExtension.java

@@ -0,0 +1,126 @@
+/*
+Copyright 2025-2026 Prospect Robotics SWENext Club
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+package com.team2813;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.RuntimeType;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import org.junit.jupiter.api.extension.*;
+
+/**
+ * JUnit Jupiter extension for testing code that depends on WPILib.
+ *
+ * <p>Also provides a {@link CommandTester} for tests.
+ *
+ * <p>Example use:
+ *
+ * <pre>{@code
+ * @ExtendWith(WPILibExtension.class)
+ * public final class FlightSubsystemTest {
+ *
+ *   @Test
+ *   public void initiallyNotInAir() {
+ *     var flight = new FlightSubsystem();
+ *
+ *     assertThat(flight.inAir()).isFalse();
+ *   }
+ *
+ *   @Test
+ *   public void takesFlight(CommandTester commandTester) {
+ *     var flight = new FlightSubsystem();
+ *     Command takeOff = flight.createTakeOffCommandCommand();
+ *
+ *     commandTester.runUntilComplete(takeOff);
+ *
+ *     assertThat(flight.inAir()).isTrue();
+ *   }
+ * }
+ * }</pre>
+ *
+ * @since 2.0.0
+ */
+// This is taken from [lib2813](https://github.com/Prospect-Robotics/lib2813/blob/main/testing/src/main/java/com/team2813/lib2813/testing/junit/jupiter/WPILibExtension.java).
+// When lib2813 is on maven central, this file can be deleted in favor of adding a dependency on lib2813.
+public final class WPILibExtension
+    implements Extension,
+        AfterAllCallback,
+        AfterEachCallback,
+        BeforeAllCallback,
+        ParameterResolver {
+  private static final double NANOS_PER_SECOND = 1_000_000_000d;
+
+  @Override
+  public void beforeAll(ExtensionContext context) {
+    // See https://www.chiefdelphi.com/t/driverstation-getalliance-in-gradle-test/
+    if (!HAL.initialize(500, 0)) {
+      throw new IllegalStateException("Could not initialize Hardware Abstraction Layer");
+    }
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.notifyNewData();
+    SimHooks.setHALRuntimeType(RuntimeType.kSimulation.value);
+
+    CommandScheduler commandScheduler = CommandScheduler.getInstance();
+    commandScheduler.enable();
+    commandScheduler.cancelAll();
+    commandScheduler.unregisterAllSubsystems();
+  }
+
+  @Override
+  public void afterEach(ExtensionContext context) {
+    CommandScheduler commandScheduler = CommandScheduler.getInstance();
+    commandScheduler.cancelAll();
+    commandScheduler.unregisterAllSubsystems();
+  }
+
+  @Override
+  public void afterAll(ExtensionContext context) {
+    CommandScheduler commandScheduler = CommandScheduler.getInstance();
+    commandScheduler.cancelAll();
+    commandScheduler.unregisterAllSubsystems();
+    commandScheduler.disable();
+    DriverStationSim.setEnabled(false);
+    DriverStationSim.notifyNewData();
+  }
+
+  @Override
+  public boolean supportsParameter(
+      ParameterContext parameterContext, ExtensionContext extensionContext)
+      throws ParameterResolutionException {
+    return CommandTester.class.equals(parameterContext.getParameter().getType());
+  }
+
+  @Override
+  public CommandTester resolveParameter(
+      ParameterContext parameterContext, ExtensionContext extensionContext) {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+
+    return command -> {
+      SimHooks.pauseTiming();
+      try {
+        scheduler.schedule(command);
+        do {
+          scheduler.run();
+          SimHooks.stepTiming(TimedRobot.kDefaultPeriod);
+        } while (scheduler.isScheduled(command));
+      } finally {
+        SimHooks.resumeTiming();
+      }
+    };
+  }
+}