feat: create elevator tests

Signed-off-by: Dasun Abeykoon <[email protected]>

Author: dabeycorn

src/main/java/org/frc6423/robot/subsystems/superstructure/elevator/Elevator.java

@@ -33,7 +33,7 @@
 import java.util.function.DoubleSupplier;
 
 /** Elevator Subsystem */
-public class Elevator extends SubsystemBase {
+public class Elevator extends SubsystemBase implements AutoCloseable {
   // * CONSTANTS
   public static final double GEAR_REDUCTION = 3 / 1;
 
@@ -198,4 +198,9 @@ public Command runExtension(DoubleSupplier extensionMeters) {
   public Command runExtension(double extensionMeters) {
     return runExtension(() -> extensionMeters);
   }
+
+  @Override
+  public void close() throws Exception {
+    hardware.close();
+  }
 }

src/test/java/org/frc6423/robot/ElevatorTests.java

@@ -0,0 +1,94 @@
+// 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;
+
+import static edu.wpi.first.units.Units.Seconds;
+import static org.frc6423.lib.utilities.TestUtils.reset;
+import static org.frc6423.lib.utilities.TestUtils.runToCompletion;
+import static org.frc6423.lib.utilities.TestUtils.setupTest;
+import static org.frc6423.robot.subsystems.superstructure.elevator.Elevator.MAX_EXTENSION_HEIGHT;
+
+import edu.wpi.first.units.measure.Time;
+import edu.wpi.first.wpilibj.Timer;
+import java.util.ArrayList;
+import java.util.stream.Stream;
+import org.frc6423.robot.subsystems.superstructure.elevator.Elevator;
+import org.frc6423.robot.subsystems.superstructure.elevator.ElevatorExtension;
+import org.frc6423.robot.subsystems.superstructure.elevator.ElevatorIOSim;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.RepeatedTest;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+/** {@link Elevator} subsystem test class */
+class ElevatorTests {
+  /** The max time a test can run for before auto failing */
+  static final Time TIMEOUT = Seconds.of(3.0);
+
+  Elevator elevator;
+  Timer timer = new Timer();
+
+  @BeforeEach
+  public void init() {
+    // Init sys
+    setupTest();
+    // Create elevator
+    elevator = new Elevator(new ElevatorIOSim());
+    // Start test timer
+    timer.start();
+  }
+
+  @AfterEach
+  public void destroy() throws Exception {
+    // Stop and record test time
+    timer.stop();
+    System.out.println("FINISHED TEST IN " + timer.get() + " SECONDS");
+
+    // Reset elevator
+    reset(elevator);
+  }
+
+  /** Run 10 randomly generated extension heights between 0.0 and the max extension height */
+  @RepeatedTest(10)
+  public void runRandExtension() {
+    runToCompletion(
+        elevator
+            // Picks a random extension height between 0.0 and max extension height
+            .runExtension(MAX_EXTENSION_HEIGHT.times(Math.random()))
+            .until(() -> elevator.isNearSetpointPose())
+            .withTimeout(TIMEOUT));
+  }
+
+  /**
+   * Run all {@link ElevatorExtension}
+   *
+   * @param extension {@link ElevatorExtension}
+   */
+  @ParameterizedTest
+  @MethodSource("provideExtensionHeights")
+  public void runExtensions(ElevatorExtension extension) {
+    runToCompletion(
+        elevator
+            .runExtension(extension)
+            .until(() -> elevator.isNearSetpointPose())
+            .withTimeout(TIMEOUT));
+  }
+
+  /**
+   * @return {@link Stream} of all {@link ElevatorExtension}
+   */
+  private static Stream<Arguments> provideExtensionHeights() {
+    ArrayList<Arguments> extensions = new ArrayList<>();
+    for (var extension : ElevatorExtension.values()) {
+      extensions.add(Arguments.of(extension));
+    }
+
+    return extensions.stream();
+  }
+}