diff --git a/build.gradle b/build.gradle index 7a8b96b..1d18eb8 100644 --- a/build.gradle +++ b/build.gradle @@ -1,5 +1,6 @@ plugins { id "java" + id "jacoco" id "edu.wpi.first.GradleRIO" version "2024.2.1" id "com.diffplug.spotless" version "6.23.3" } @@ -8,7 +9,7 @@ spotless { java { target fileTree('.') { include '**/*.java' - exclude '**/build/**', '**/build-*/**' + exclude '**/build/**', '**/build-*/**', '**/logs/**' } toggleOffOn() googleJavaFormat('1.19.1') @@ -25,6 +26,19 @@ java { targetCompatibility = JavaVersion.VERSION_17 } +jacoco { + toolVersion = "0.8.11" + reportsDirectory = layout.projectDirectory.dir("logs/jacoco") +} + +jacocoTestReport { + reports { + xml.required = false + csv.required = false + html.outputLocation = layout.projectDirectory.dir("logs/jacocoHtml") + } +} + def ROBOT_MAIN_CLASS = "frc.robot.Main" // Define my targets (RoboRIO) and artifacts (deployable files) @@ -91,6 +105,11 @@ dependencies { test { useJUnitPlatform() systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' + finalizedBy jacocoTestReport // report is always generated after tests run +} + +jacocoTestReport { + dependsOn test // tests are required to run before generating the report } // Simulation configuration (e.g. environment variables). diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 530e0bc..2621e96 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,7 +20,6 @@ import frc.robot.Constants.IntakeConstants; import frc.robot.commands.ArmIdleCommand; import frc.robot.commands.BasicIntakeCommand; -import frc.robot.commands.ReallySimpleAuto; import frc.robot.commands.ShootToAmpCommand; import frc.robot.commands.SmartIntakeCommand; import frc.robot.commands.SmartShootCommand; @@ -58,9 +57,6 @@ public RobotContainer() { "Shoot To Shooter", new SmartShootCommand(shooterSubsystem, intakeSubsystem, armSubsystem, driveSubsystem)); PhotonCameraSystem.getAprilTagWithID(0); // Load the class before enable. - autoChooser.addOption( - "Shoot & Back", - new ReallySimpleAuto(armSubsystem, intakeSubsystem, shooterSubsystem, driveSubsystem)); SmartDashboard.putData("Auto Chooser", autoChooser); if (RobotBase.isSimulation()) { simInit(); diff --git a/src/main/java/frc/robot/commands/BasicRunShooterCommand.java b/src/main/java/frc/robot/commands/BasicRunShooterCommand.java index b571506..10576b3 100644 --- a/src/main/java/frc/robot/commands/BasicRunShooterCommand.java +++ b/src/main/java/frc/robot/commands/BasicRunShooterCommand.java @@ -1,21 +1,25 @@ package frc.robot.commands; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants.ShooterConstants; import frc.robot.commands.led_commands.LEDLoadingWaitCommand; import frc.robot.subsystems.ShooterSubsystem; public class BasicRunShooterCommand extends SequentialCommandGroup { - BasicRunShooterCommand(ShooterSubsystem shooterSubsystem) { + public BasicRunShooterCommand(ShooterSubsystem shooterSubsystem) { super( shooterSubsystem.run( () -> shooterSubsystem.setShooterSpeed(ShooterConstants.kShooterSpeed))); } - BasicRunShooterCommand(ShooterSubsystem shooterSubsystem, double untilTimeSeconds) { + public BasicRunShooterCommand(ShooterSubsystem shooterSubsystem, double untilTimeSeconds) { super( - shooterSubsystem - .run(() -> shooterSubsystem.setShooterSpeed(ShooterConstants.kShooterSpeed)) - .raceWith(new LEDLoadingWaitCommand(untilTimeSeconds))); + new ParallelRaceGroup( + new BasicRunShooterCommand(shooterSubsystem), // Run the shooter + new LEDLoadingWaitCommand(untilTimeSeconds)), + shooterSubsystem.runOnce( + () -> shooterSubsystem.setShooterSpeed(0) // Stop the shooter after the wait + )); } } diff --git a/src/main/java/frc/robot/commands/MoveArmToAmp.java b/src/main/java/frc/robot/commands/MoveArmToAmp.java index b0c6383..3f64182 100644 --- a/src/main/java/frc/robot/commands/MoveArmToAmp.java +++ b/src/main/java/frc/robot/commands/MoveArmToAmp.java @@ -4,7 +4,7 @@ import frc.robot.subsystems.ArmSubsystem; public class MoveArmToAmp extends SequentialCommandGroup { - MoveArmToAmp(ArmSubsystem armSubsystem) { + public MoveArmToAmp(ArmSubsystem armSubsystem) { super( armSubsystem .run(() -> armSubsystem.setArmToPosition(0.5)) diff --git a/src/main/java/frc/robot/commands/ReallySimpleAuto.java b/src/main/java/frc/robot/commands/ReallySimpleAuto.java deleted file mode 100644 index efc747b..0000000 --- a/src/main/java/frc/robot/commands/ReallySimpleAuto.java +++ /dev/null @@ -1,20 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.subsystems.ArmSubsystem; -import frc.robot.subsystems.DriveSubsystem; -import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.subsystems.ShooterSubsystem; - -public class ReallySimpleAuto extends SequentialCommandGroup { - public ReallySimpleAuto( - ArmSubsystem armSubsystem, - IntakeSubsystem intakeSubsystem, - ShooterSubsystem shooterSubsystem, - DriveSubsystem driveSubsystem) { - super( - new SmartShootCommand(shooterSubsystem, intakeSubsystem, armSubsystem, driveSubsystem), - driveSubsystem.run(() -> driveSubsystem.drive(-0.25, 0, 0)).raceWith(new WaitCommand(2.5))); - } -} diff --git a/src/main/java/frc/robot/commands/VibrateControllerCommand.java b/src/main/java/frc/robot/commands/VibrateControllerCommand.java index 91821e8..818a952 100644 --- a/src/main/java/frc/robot/commands/VibrateControllerCommand.java +++ b/src/main/java/frc/robot/commands/VibrateControllerCommand.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.commands.util_commands.RepeatForCommand; public class VibrateControllerCommand extends SequentialCommandGroup { public VibrateControllerCommand( @@ -24,16 +25,13 @@ public VibrateControllerCommand(XboxController controller, int repetitions) { private static Command createVibrateControllerCommand( XboxController controller, int repetitions, double intensity, double duration) { - Command command = - new InstantCommand(() -> controller.setRumble(RumbleType.kBothRumble, intensity)); - for (int i = 0; i < repetitions; i++) { - command = - command - .andThen(new WaitCommand(duration)) - .andThen(() -> controller.setRumble(RumbleType.kBothRumble, 0)) - .andThen(new WaitCommand(duration)) - .andThen(() -> controller.setRumble(RumbleType.kBothRumble, intensity)); - } - return command.finallyDo(() -> controller.setRumble(RumbleType.kBothRumble, 0)); + SequentialCommandGroup toRepeatCommand = + new SequentialCommandGroup( + new InstantCommand(() -> controller.setRumble(RumbleType.kBothRumble, intensity)), // + new WaitCommand(duration), + new InstantCommand(() -> controller.setRumble(RumbleType.kBothRumble, 0)), + new WaitCommand(duration)); + + return new RepeatForCommand(toRepeatCommand, repetitions); } } diff --git a/src/main/java/frc/robot/commands/led_commands/LEDLoadingWaitCommand.java b/src/main/java/frc/robot/commands/led_commands/LEDLoadingWaitCommand.java index 7b35078..b275cc4 100644 --- a/src/main/java/frc/robot/commands/led_commands/LEDLoadingWaitCommand.java +++ b/src/main/java/frc/robot/commands/led_commands/LEDLoadingWaitCommand.java @@ -7,6 +7,7 @@ public class LEDLoadingWaitCommand extends WaitCommand { private double seconds; private Color colorToFill; + public static final Color DEFAULT_COLOR = new Color(0, 200, 255); public LEDLoadingWaitCommand(double seconds, Color colorToFill) { super(seconds); @@ -16,7 +17,7 @@ public LEDLoadingWaitCommand(double seconds, Color colorToFill) { } public LEDLoadingWaitCommand(double seconds) { - this(seconds, new Color(0, 200, 255)); + this(seconds, DEFAULT_COLOR); } @Override diff --git a/src/main/java/frc/robot/commands/util_commands/RepeatForCommand.java b/src/main/java/frc/robot/commands/util_commands/RepeatForCommand.java new file mode 100644 index 0000000..07f8636 --- /dev/null +++ b/src/main/java/frc/robot/commands/util_commands/RepeatForCommand.java @@ -0,0 +1,35 @@ +package frc.robot.commands.util_commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RepeatCommand; + +public class RepeatForCommand extends RepeatCommand { + private double count; + private Command command; + + /** + * Repeats a command for a specified number of times + * + * @param command The command to repeat + * @param count The number of times to repeat the command + */ + public RepeatForCommand(Command command, int count) { + super(command); + this.command = command; + this.count = count; + } + + @Override + public void execute() { + // only decrement the count if the command has finished + if (command.isFinished()) { + count--; + } + super.execute(); + } + + @Override + public boolean isFinished() { + return count <= 0; + } +} diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index be10b1b..1c55235 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -16,7 +16,7 @@ import frc.utils.ExtraFunctions; import frc.utils.sim_utils.CANSparkMAXWrapped; -public class ArmSubsystem extends SubsystemBase { +public class ArmSubsystem extends SubsystemBase implements AutoCloseable { // We only have RelativeEncoder for now. Its better than nothing. private final CANSparkMAXWrapped arm; private final CANSparkMAXWrapped armFollower; @@ -63,6 +63,12 @@ private void setupSparkMax() { arm.burnFlash(); } + @Override + public void close() { + arm.close(); + armFollower.close(); + } + /** * Sets the arm to a position. This is a wrapper for the Spark Max's set method. * diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 8d71e87..9d371f1 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -20,6 +20,7 @@ public class IntakeSubsystem extends SubsystemBase implements AutoCloseable { private final CANSparkMAXWrapped armIntake; private final CANSparkMAXWrapped groundIntake; private final ColorSensorV3Wrapped colorSensor; + private final Thread fastColorCheckThread; private boolean isForced = false; public IntakeSubsystem() { @@ -29,14 +30,16 @@ public IntakeSubsystem() { setupIntakeMotors(); colorSensor = new ColorSensorV3Wrapped(ColorSensorConstants.kColorSensorPort); - new Thread( + fastColorCheckThread = + new Thread( () -> { while (true) { fastPeriodic(); } }, - "Fast Color Check Loop") - .start(); + "Fast Color Check Loop"); + fastColorCheckThread.setDaemon(true); + fastColorCheckThread.start(); } private void setupIntakeMotors() { @@ -57,6 +60,7 @@ private void setupIntakeMotors() { @Override public void close() { + fastColorCheckThread.interrupt(); armIntake.close(); groundIntake.close(); colorSensor.close(); @@ -127,7 +131,7 @@ public void periodic() { SmartDashboard.putNumber("ColorSensor - Blue", colorSensor.getBlue()); SmartDashboard.putNumber("ColorSensor - IR", colorSensor.getIR()); SmartDashboard.putBoolean("Note Detected", hasNote()); - checkIfHasNote(); + // don't check for a note here cause it breaks the unit tests SOMEHOW! } private void fastPeriodic() { diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 43e4b6e..9b22f41 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -7,7 +7,7 @@ import frc.robot.Constants.LEDConstants; import frc.utils.BetterLED; -public class LEDSubsystem extends SubsystemBase { +public class LEDSubsystem extends SubsystemBase implements AutoCloseable { BetterLED strip; Color lastSetColor = new Color(); Color lastSetBlinkingColor = new Color(); @@ -22,6 +22,15 @@ public void periodic() { SmartDashboard.putString("LED Currently Running", strip.getCurrentCommandName()); } + @Override + public void close() { + strip.close(); + } + + public int getLedCount() { + return strip.getLedCount(); + } + public void fill(Color color) { if (lastSetColor.equals(color)) { return; @@ -68,13 +77,17 @@ public void setStatusColor(boolean status) { } } - public void blink(Color color) { + public void blink(Color color, double delaySeconds) { if (lastSetBlinkingColor.equals(color)) { return; } lastSetColor = new Color(); // Reset back to 0 lastSetBlinkingColor = color; - strip.blink(color); + strip.blink(color, delaySeconds); + } + + public void blink(Color color) { + blink(color, 0.2); } public void blinkRed() { @@ -84,6 +97,12 @@ public void blinkRed() { blink(new Color(255, 0, 0)); } + /** + * Fills the led's until the given percentage. + * + * @param percentage a value between 0 and 1 + * @param color the color to fill with + */ public void fillPercentageWithColor(double percentage, Color color) { fill(color, (int) (strip.getLedCount() * percentage), true); } diff --git a/src/main/java/frc/robot/subsystems/LEDSystem.java b/src/main/java/frc/robot/subsystems/LEDSystem.java index ec07193..c31cb17 100644 --- a/src/main/java/frc/robot/subsystems/LEDSystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSystem.java @@ -3,7 +3,7 @@ import edu.wpi.first.wpilibj2.command.Command; public final class LEDSystem { - private static final LEDSubsystem ledSubsystemInstance = new LEDSubsystem(); + private static LEDSubsystem ledSubsystemInstance = new LEDSubsystem(); private LEDSystem() {} @@ -18,4 +18,17 @@ public static Command run(Runnable toRun) { public static Command getBlinkRedCommand() { return ledSubsystemInstance.getRedBlinkCommand(); } + + public static void resetLEDSubsystem() { + if (ledSubsystemInstance == null) { + ledSubsystemInstance = new LEDSubsystem(); + return; + } + ledSubsystemInstance.close(); + ledSubsystemInstance = new LEDSubsystem(); + } + + public static void changeLEDSubsystemInstance(LEDSubsystem ledSubsystem) { + ledSubsystemInstance = ledSubsystem; + } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 28c20fc..7116e7e 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -3,13 +3,18 @@ import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class ShooterSubsystem extends SubsystemBase { +public class ShooterSubsystem extends SubsystemBase implements AutoCloseable { private final Spark shooterMotor; public ShooterSubsystem() { shooterMotor = new Spark(0); } + @Override + public void close() { + shooterMotor.close(); + } + public void setShooterSpeed(double speed) { shooterMotor.set(speed); } diff --git a/src/main/java/frc/utils/BetterLED.java b/src/main/java/frc/utils/BetterLED.java index d7d7976..76f6c6c 100644 --- a/src/main/java/frc/utils/BetterLED.java +++ b/src/main/java/frc/utils/BetterLED.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.util.Color; import java.util.ArrayList; +import java.util.concurrent.locks.ReentrantLock; public class BetterLED extends AddressableLED { public enum AnimationType { // Is already static. @@ -20,6 +21,9 @@ public enum AnimationType { // Is already static. private int rainbowFirstPixelHue = 0; private int currentAnimationIndex = 0; + private ReentrantLock ledMutex = new ReentrantLock(true); + private volatile boolean isThreadKilled = false; + // The call list will have all animation calls in it, // This will run like a scheduler, just on a smaller scale. // it will probably have only a single function, but is a an Arraylist just in case. @@ -40,20 +44,59 @@ public BetterLED(int port, int ledCount) { } private void setupThread() { + isThreadKilled = false; // Reset the kill flag on setup of thread ledUpdateThread = new Thread( () -> { for (; ; ) { // Infinity call periodic, - periodic(); + ledMutex.lock(); + try { + periodic(); + } finally { + ledMutex.unlock(); + } Timer.delay(0.015); - // Maybe add a delay? - // or should the delay be handled by the respective functions? + + // Kill the thread if it has ben set to kill. + if (isThreadKilled) { + break; + } } }, "LED Control Thread"); + ledUpdateThread.setDaemon(true); ledUpdateThread.start(); } + @Override + public void close() { + isThreadKilled = true; + super.close(); + } + + /** + * Check if the thread is alive and if not, restart it. + * + * @return thread got restarted + *

true if restarted, false if not + */ + public boolean checkThreadStatus() { + if (!ledUpdateThread.isAlive()) { + setupThread(); // restart thread + return true; + } + return false; + } + + /** + * Returns the buffer thats used. You should never need to use this except debugging or similar. + * + * @return The main buffer used by the LED instance. + */ + public AddressableLEDBuffer getBuffer() { + return mainBuffer; + } + public int getLedCount() { return ledCount; } @@ -186,19 +229,34 @@ public void setLength(int length) { } private void changeLoopTo(LEDCommand func) { - commandList.clear(); // Clear the list first - commandList.add(func); // then add the runnable + ledMutex.lock(); + try { + commandList.clear(); // Clear the list first + commandList.add(func); // then add the runnable + } finally { + ledMutex.unlock(); + } } public void addToLoop(LEDCommand command) { - commandList.add(command); + ledMutex.lock(); + try { + commandList.add(command); + } finally { + ledMutex.unlock(); + } } public void removeFromLoop(int amount) { - for (int i = 0; i < amount; i++) { - if (!commandList.isEmpty()) { - commandList.remove(0); + ledMutex.lock(); + try { + for (int i = 0; i < amount; i++) { + if (!commandList.isEmpty()) { + commandList.remove(0); + } } + } finally { + ledMutex.unlock(); } } diff --git a/src/main/java/frc/utils/sim_utils/CANSparkMAXWrapped.java b/src/main/java/frc/utils/sim_utils/CANSparkMAXWrapped.java index 9c4db6b..da19271 100644 --- a/src/main/java/frc/utils/sim_utils/CANSparkMAXWrapped.java +++ b/src/main/java/frc/utils/sim_utils/CANSparkMAXWrapped.java @@ -18,4 +18,8 @@ public void close() { } super.close(); } + + public boolean isThisClosed() { + return isClosed.get(); + } } diff --git a/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java b/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java index 89480c7..37bcfa1 100644 --- a/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java +++ b/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java @@ -5,7 +5,7 @@ import edu.wpi.first.wpilibj.RobotBase; public class ColorSensorV3Wrapped extends ColorSensorV3 implements AutoCloseable { - private static ColorSensorV3 colorSensor = null; + private static ColorSensorV3 colorSensor; private static final int[] rgbd = new int[4]; // Red, Green, Blue, and Distance public ColorSensorV3Wrapped(I2C.Port port) { diff --git a/src/test/java/SparkMAXWrapperTest.java b/src/test/java/SparkMAXWrapperTest.java deleted file mode 100644 index b099f97..0000000 --- a/src/test/java/SparkMAXWrapperTest.java +++ /dev/null @@ -1,24 +0,0 @@ -import static org.junit.jupiter.api.Assertions.assertEquals; - -import edu.wpi.first.hal.HAL; -import frc.utils.sim_utils.CANSparkMAXWrapped; -import frc.utils.sim_utils.SparkMAXSimAddon; -import org.junit.jupiter.api.BeforeEach; -import org.junit.jupiter.api.Test; - -class SparkMAXWrapperTest { - @BeforeEach - public void setUp() { - HAL.initialize(500, 0); // init HAL just in case - } - - @Test - void testSparkMAXWrapper() { - for (int i = 0; i < 50; i++) { - CANSparkMAXWrapped sparkMAX = - new CANSparkMAXWrapped(i, CANSparkMAXWrapped.MotorType.kBrushless); - assertEquals(sparkMAX, SparkMAXSimAddon.getSparkMAX(i)); - sparkMAX.close(); - } - } -} diff --git a/src/test/java/command_tests/LEDIdleCommandTest.java b/src/test/java/command_tests/LEDIdleCommandTest.java new file mode 100644 index 0000000..754da1f --- /dev/null +++ b/src/test/java/command_tests/LEDIdleCommandTest.java @@ -0,0 +1,65 @@ +package command_tests; + +import static subsystem_tests.led_tests.utils.LEDTestUtils.checkForColorInAll; + +import command_tests.utils.CommandTestBase; +import edu.wpi.first.hal.HAL; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import edu.wpi.first.wpilibj.util.Color; +import frc.robot.commands.led_commands.LEDIdleCommand; +import frc.robot.subsystems.IntakeSubsystem; +import frc.utils.sim_utils.ColorSensorV3Wrapped; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +class LEDIdleCommandTest extends CommandTestBase { + private IntakeSubsystem intakeSubsystem; + + @BeforeEach + public void setUp() { + super.setUp(); + HAL.initialize(500, 0); + intakeSubsystem = new IntakeSubsystem(); + } + + @AfterEach + public void tearDown() { + super.tearDown(); + intakeSubsystem.close(); + } + + @Test + void colorOnDisabled() { + DriverStationSim.setEnabled(false); + DriverStationSim.setDsAttached(true); + DriverStationSim.notifyNewData(); // ! Breaks without this + commandScheduler.schedule( + new LEDIdleCommand(ledSubsystem, intakeSubsystem).ignoringDisable(true)); + commandScheduler.run(); + Timer.delay(0.1); // let led thread do its thing + checkForColorInAll(ledSubsystem, Color.kOrangeRed, "Color Should be orange when disabled"); + } + + @Test + void colorOnEnabled() { + DriverStationSim.setDsAttached(true); + DriverStationSim.setEnabled(true); + DriverStationSim.notifyNewData(); // ! Breaks without this + commandScheduler.schedule( + new LEDIdleCommand(ledSubsystem, intakeSubsystem).ignoringDisable(true)); + + // * check without Note + ColorSensorV3Wrapped.setRGBD(0, 0, 0, 0); + commandScheduler.run(); + Timer.delay(0.1); // let led thread do its thing + checkForColorInAll(ledSubsystem, Color.kRed, "Color should be red when no Note is detected"); + + // * check with Note + ColorSensorV3Wrapped.setRGBD(2500, 0, 0, 900); + commandScheduler.run(); + Timer.delay(0.1); // let led thread do its thing + checkForColorInAll(ledSubsystem, Color.kGreen, "Color should be green when Note is detected"); + } +} diff --git a/src/test/java/command_tests/arm_tests/BasicIntakeTest.java b/src/test/java/command_tests/arm_tests/BasicIntakeTest.java new file mode 100644 index 0000000..e73f538 --- /dev/null +++ b/src/test/java/command_tests/arm_tests/BasicIntakeTest.java @@ -0,0 +1,66 @@ +package command_tests.arm_tests; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; +import static subsystem_tests.led_tests.utils.LEDTestUtils.checkForColorInAll; + +import com.revrobotics.CANSparkMax; +import command_tests.utils.CommandTestBase; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.util.Color; +import frc.robot.Constants.IntakeConstants; +import frc.robot.commands.BasicIntakeCommand; +import frc.robot.subsystems.IntakeSubsystem; +import frc.utils.sim_utils.ColorSensorV3Wrapped; +import frc.utils.sim_utils.SparkMAXSimAddon; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +class BasicIntakeTest extends CommandTestBase { + private IntakeSubsystem intakeSubsystem; + + private CANSparkMax intakeMotor; + private BasicIntakeCommand intakeCommand; + + @BeforeEach + public void setUp() { + super.setUp(); + intakeSubsystem = new IntakeSubsystem(); + intakeMotor = SparkMAXSimAddon.getSparkMAX(IntakeConstants.kArmIntakeMotorCanID); + intakeCommand = new BasicIntakeCommand(intakeSubsystem); + + commandScheduler.schedule(intakeCommand); + } + + @AfterEach + public void tearDown() { + intakeSubsystem.close(); + super.tearDown(); + } + + @Test + void testSpeed() { + commandScheduler.run(); + // Motor shouldn't be 0 when intaking + assertNotEquals(0, intakeMotor.get()); + } + + @Test + void testLEDBlinking() { + commandScheduler.run(); + Timer.delay(0.2); // let led loop do its thing + checkForColorInAll(ledSubsystem, Color.kRed, "Color should be red when started"); + Timer.delay(0.2); + checkForColorInAll(ledSubsystem, Color.kBlack, "Color should be closed when blinking red"); + } + + @Test + void testCommandEnds() { + commandScheduler.run(); + ColorSensorV3Wrapped.setRGBD(2500, 0, 0, 900); + commandScheduler.run(); + assertEquals(true, intakeCommand.isFinished(), "Command should be finished when ending"); + assertEquals(0, intakeMotor.get(), "Motor should stop after detecting color"); + } +} diff --git a/src/test/java/command_tests/arm_tests/BasicShooterTest.java b/src/test/java/command_tests/arm_tests/BasicShooterTest.java new file mode 100644 index 0000000..0f704bf --- /dev/null +++ b/src/test/java/command_tests/arm_tests/BasicShooterTest.java @@ -0,0 +1,64 @@ +package command_tests.arm_tests; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static subsystem_tests.led_tests.utils.LEDTestUtils.testAtTime; + +import command_tests.utils.CommandTestBase; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.PWMSim; +import frc.robot.Constants.ShooterConstants; +import frc.robot.commands.BasicRunShooterCommand; +import frc.robot.subsystems.ShooterSubsystem; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +class BasicShooterTest extends CommandTestBase { + private ShooterSubsystem shooterSubsystem; + private PWMSim shooterMotor; + + private static final double kWaitTime = 2; + + @BeforeEach + public void setUp() { + super.setUp(); + shooterSubsystem = new ShooterSubsystem(); + shooterMotor = new PWMSim(ShooterConstants.kShooterMotorPwmID); + BasicRunShooterCommand basicShooterCommand = + new BasicRunShooterCommand(shooterSubsystem, kWaitTime); + + commandScheduler.schedule(basicShooterCommand); + } + + @AfterEach + public void tearDown() { + super.tearDown(); + shooterSubsystem.close(); + } + + @Test + void testSetSpeed() { + commandScheduler.run(); + // Motor should be at the set speed + assertEquals( + ShooterConstants.kShooterSpeed, + shooterMotor.getSpeed(), + "Shooter motor should be at shooter speed"); + Timer.delay(kWaitTime); + commandScheduler.run(); + // Motor should be at 0 after waitTime + assertEquals( + 0, + shooterMotor.getSpeed(), + "Shooter motor should be at 0 after %s seconds".formatted(kWaitTime)); + } + + @Test + void testLEDLoading() { + double startTime = Timer.getFPGATimestamp(); + commandScheduler.run(); + Timer.delay(kWaitTime); // load waitTime + commandScheduler.run(); + testAtTime(ledSubsystem, startTime, kWaitTime); + } +} diff --git a/src/test/java/command_tests/arm_tests/MoveArmToAmpTest.java b/src/test/java/command_tests/arm_tests/MoveArmToAmpTest.java new file mode 100644 index 0000000..72442da --- /dev/null +++ b/src/test/java/command_tests/arm_tests/MoveArmToAmpTest.java @@ -0,0 +1,40 @@ +package command_tests.arm_tests; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import command_tests.utils.CommandTestBase; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.commands.MoveArmToAmp; +import frc.robot.subsystems.ArmSubsystem; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +class MoveArmToAmpTest extends CommandTestBase { + private MoveArmToAmp moveArmToAmpCommand; + private ArmSubsystem armSubsystem; + + @BeforeEach + public void setUp() { + super.setUp(); + + armSubsystem = new ArmSubsystem(); + moveArmToAmpCommand = new MoveArmToAmp(armSubsystem); + commandScheduler.schedule(moveArmToAmpCommand); + } + + @AfterEach + public void tearDown() { + super.tearDown(); + armSubsystem.close(); + } + + @Test + void testArmSetpointIsTrue() { + commandScheduler.run(); + double setpoint = SmartDashboard.getNumber("Setpoint", -1); + + // !if this value 0.5 is changed or moved to a constant instead (it should be) change this + assertEquals(0.5, setpoint, "Arm should have been on 0.5 (meaning the amp)"); + } +} diff --git a/src/test/java/command_tests/controller_commands/VibrateControllerCommandTest.java b/src/test/java/command_tests/controller_commands/VibrateControllerCommandTest.java new file mode 100644 index 0000000..5ac6954 --- /dev/null +++ b/src/test/java/command_tests/controller_commands/VibrateControllerCommandTest.java @@ -0,0 +1,79 @@ +package command_tests.controller_commands; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import command_tests.utils.CommandTestBase; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.simulation.XboxControllerSim; +import frc.robot.commands.VibrateControllerCommand; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +class VibrateControllerCommandTest extends CommandTestBase { + private VibrateControllerCommand vibrateControllerCommand; + private XboxControllerSim controllerSim; + + private static final double kDelta = 0.01; + + private static final int kRepetitions = 3; + private static final double kIntensity = 0.5; // 0.5 due to a bug in the simulator + private static final double kWaitTime = 1.5; + + @BeforeEach + public void setUp() { + super.setUp(); + + XboxController controller = new XboxController(1); + controllerSim = new XboxControllerSim(1); + vibrateControllerCommand = + new VibrateControllerCommand(controller, kRepetitions, kIntensity, kWaitTime); + commandScheduler.schedule(vibrateControllerCommand); + } + + @AfterEach + public void tearDown() { + super.tearDown(); + } + + @Test + void testItVibrates() { + commandScheduler.run(); + assertEquals(kIntensity, controllerSim.getRumble(RumbleType.kLeftRumble), kDelta); + } + + @Test + void testItStopsVibrating() { + commandScheduler.run(); + Timer.delay(kWaitTime); + commandScheduler.run(); + assertEquals(0, controllerSim.getRumble(RumbleType.kLeftRumble), kDelta); + } + + @Test + void testItRepeats() { + // run command scheduler twice to end wait command and start vibration command + for (int i = 0; i < kRepetitions; i++) { + commandScheduler.run(); + commandScheduler.run(); + assertEquals( + kIntensity, + controllerSim.getRumble(RumbleType.kLeftRumble), + kDelta, + "controller should vibrate at index: " + i); + Timer.delay(kWaitTime + 0.1); + commandScheduler.run(); + commandScheduler.run(); + assertEquals( + 0, + controllerSim.getRumble(RumbleType.kLeftRumble), + kDelta, + "controller should stop vibrating at index: " + i); + Timer.delay(kWaitTime + 0.1); + commandScheduler.run(); + commandScheduler.run(); + } + } +} diff --git a/src/test/java/command_tests/simple_command_tests/LEDWaitingCommandTest.java b/src/test/java/command_tests/simple_command_tests/LEDWaitingCommandTest.java new file mode 100644 index 0000000..0fff8fc --- /dev/null +++ b/src/test/java/command_tests/simple_command_tests/LEDWaitingCommandTest.java @@ -0,0 +1,64 @@ +package command_tests.simple_command_tests; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static subsystem_tests.led_tests.utils.LEDTestUtils.testAtTime; + +import command_tests.utils.CommandTestBase; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.commands.led_commands.LEDLoadingWaitCommand; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +class LEDWaitingCommandTest extends CommandTestBase { + private static final double kWaitTime = 2.0; + private LEDLoadingWaitCommand ledLoadingWaitCommand; + + @BeforeEach + public void setUp() { + super.setUp(); + ledLoadingWaitCommand = new LEDLoadingWaitCommand(kWaitTime); + commandScheduler.schedule(ledLoadingWaitCommand); + } + + @AfterEach + public void tearDown() { + super.tearDown(); + } + + void testLEDLoading(double waitTime) { + double startTime = Timer.getFPGATimestamp(); + for (double i = 0.1; Timer.getFPGATimestamp() - startTime + i < waitTime; i += 0.1) { + Timer.delay(i); + commandScheduler.run(); + testAtTime(ledSubsystem, startTime, waitTime); + } + } + + /** + * This test takes a lot of time as it tries a lot of differently timed {@link + * LEDLoadingWaitCommand}s + */ + @Test + void testLEDLoading() { + // try multiple wait times, just so we can do it + for (double i = 0.0; i <= 6; i += 1.2) { + commandScheduler.cancelAll(); + commandScheduler.schedule(new LEDLoadingWaitCommand(i)); + commandScheduler.run(); + testLEDLoading(i); + } + } + + @Test + void itActuallyEnds() { + commandScheduler.run(); + assertEquals( + false, + ledLoadingWaitCommand.isFinished(), + "led wait command shouldn't finish before wait time"); + Timer.delay(kWaitTime); // just in case add a little bit extra time + assertEquals( + true, ledLoadingWaitCommand.isFinished(), "led wait command should finish after wait time"); + } +} diff --git a/src/test/java/command_tests/simple_command_tests/WaitANDConditionCommandTest.java b/src/test/java/command_tests/simple_command_tests/WaitANDConditionCommandTest.java new file mode 100644 index 0000000..210018c --- /dev/null +++ b/src/test/java/command_tests/simple_command_tests/WaitANDConditionCommandTest.java @@ -0,0 +1,61 @@ +package command_tests.simple_command_tests; + +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import command_tests.utils.CommandTestBase; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.commands.WaitANDConditionCommand; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +class WaitANDConditionCommandTest extends CommandTestBase { + private WaitANDConditionCommand waitConditionCommand; + private static double kWaitTime = 2.0; + private boolean condition; + + @BeforeEach + public void setUp() { + super.setUp(); + + condition = false; + + waitConditionCommand = new WaitANDConditionCommand(kWaitTime, () -> condition); + commandScheduler.schedule(waitConditionCommand); + } + + @AfterEach + public void tearDown() { + super.tearDown(); + } + + @Test + void testItCompletesOnCondition() { + commandScheduler.run(); + condition = true; + commandScheduler.run(); + assertFalse( + waitConditionCommand.isFinished(), + "command should not finish after condition true without time finish"); + } + + @Test + void testItCompletesAfterTime() { + commandScheduler.run(); + Timer.delay(kWaitTime); + commandScheduler.run(); + assertFalse( + waitConditionCommand.isFinished(), + "Command should not finish after time finishes without condition"); + } + + @Test + void testItFinishesAfterAll() { + commandScheduler.run(); + condition = true; + Timer.delay(kWaitTime); + commandScheduler.run(); + assertTrue(waitConditionCommand.isFinished(), "Command should finish after time and condition"); + } +} diff --git a/src/test/java/command_tests/utils/CommandTestBase.java b/src/test/java/command_tests/utils/CommandTestBase.java new file mode 100644 index 0000000..ad1c49d --- /dev/null +++ b/src/test/java/command_tests/utils/CommandTestBase.java @@ -0,0 +1,35 @@ +package command_tests.utils; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.subsystems.LEDSubsystem; +import frc.robot.subsystems.LEDSystem; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; + +public class CommandTestBase { + protected CommandScheduler commandScheduler; + protected LEDSubsystem ledSubsystem; + + @BeforeEach + protected void setUp() { + assert HAL.initialize(500, 0); + commandScheduler = CommandScheduler.getInstance(); + ledSubsystem = LEDSystem.getInstance(); + + // Enable robot for commands to run + DriverStationSim.setEnabled(true); + DriverStationSim.setDsAttached(true); + DriverStationSim.notifyNewData(); // ! Breaks without this + } + + @AfterEach + protected void tearDown() { + LEDSystem.resetLEDSubsystem(); + commandScheduler.cancelAll(); + commandScheduler.unregisterAllSubsystems(); // ! breaks all test tests if not done + commandScheduler.close(); + HAL.shutdown(); + } +} diff --git a/src/test/java/IntakeTests.java b/src/test/java/subsystem_tests/arm_tests/IntakeTests.java similarity index 96% rename from src/test/java/IntakeTests.java rename to src/test/java/subsystem_tests/arm_tests/IntakeTests.java index ad8b5bc..10714e0 100644 --- a/src/test/java/IntakeTests.java +++ b/src/test/java/subsystem_tests/arm_tests/IntakeTests.java @@ -1,3 +1,5 @@ +package subsystem_tests.arm_tests; + import static org.junit.jupiter.api.Assertions.assertEquals; import com.revrobotics.CANSparkMax; @@ -61,7 +63,7 @@ void testIntakeForcePushes() { ColorSensorV3Wrapped.setRGBD(2500, 0, 0, 900); intakeSubsystem.setIntakeSpeed(0.5, 0, true); // force push note out assertEquals(0.5, intakeMotor.get(), 0.001, "Intake Motor Should Run when force pushed"); - Timer.delay(0.1); + Timer.delay(0.15); // if broken, increase this assertEquals(0.5, intakeMotor.get(), 0.001, "Intake motor should continue to run when forced"); } diff --git a/src/test/java/subsystem_tests/led_tests/LedTests.java b/src/test/java/subsystem_tests/led_tests/LedTests.java new file mode 100644 index 0000000..e99a3aa --- /dev/null +++ b/src/test/java/subsystem_tests/led_tests/LedTests.java @@ -0,0 +1,133 @@ +package subsystem_tests.led_tests; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static subsystem_tests.led_tests.utils.LEDTestUtils.checkForColorInAll; +import static subsystem_tests.led_tests.utils.LEDTestUtils.getColorAtIndex; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.util.Color; +import frc.robot.subsystems.LEDSubsystem; +import frc.robot.subsystems.LEDSystem; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +class LedTests { + private LEDSubsystem ledSubsystem; + + @BeforeEach + public void setUp() { + HAL.initialize(500, 0); + ledSubsystem = LEDSystem.getInstance(); + } + + @AfterEach + public void tearDown() { + // ledSubsystem is closed by `LEDSystem.resetLEDSubsystem()` + LEDSystem.resetLEDSubsystem(); + } + + private void testUntilPercentage(double percentage) { + ledSubsystem.fillPercentageWithColor(percentage, Color.kWhite); + Timer.delay(0.1); // wait until command gets executed + for (int i = 0; i < ledSubsystem.getLedCount(); i++) { + if (i < (int) (ledSubsystem.getLedCount() * percentage)) { + assertEquals( + Color.kWhite, + getColorAtIndex(ledSubsystem, i), + "Color Should be the specified one until " + percentage); + } else { + assertEquals( + Color.kBlack, + getColorAtIndex(ledSubsystem, i), + "Color Should be Black after the specified percentage"); + } + } + } + + @Test + void testFill() { + ledSubsystem.fill(Color.kWhite); + Timer.delay(0.05); // let the loop change all the colors + for (int i = 0; i < ledSubsystem.getStrip().getLedCount(); i++) { + assertEquals( + Color.kWhite, + getColorAtIndex(ledSubsystem, i), + "Color Should be the same as filled color at led " + i); + } + } + + @Test + void testBlink() { + ledSubsystem.blink(Color.kWhite, 0.5); + Timer.delay(0.1); + checkForColorInAll( + ledSubsystem, Color.kWhite, "Starting color should have been the the color specified"); + Timer.delay(0.5); // wait for led's to close back down + checkForColorInAll( + ledSubsystem, Color.kBlack, "Color should have turned to black in blinking sequence"); + Timer.delay(0.5); + checkForColorInAll( + ledSubsystem, Color.kWhite, "Color should have turned back to the color specified"); + } + + @Test + void testBlinkRed() { + ledSubsystem.blinkRed(); + Timer.delay(0.1); + checkForColorInAll(ledSubsystem, Color.kRed, "Color should be red before blink"); + Timer.delay(0.2); + checkForColorInAll( + ledSubsystem, Color.kBlack, "Color should have turned to black in blinking sequence"); + Timer.delay(0.2); + checkForColorInAll(ledSubsystem, Color.kRed, "Color should have turned back to red"); + } + + @Test + void testFillPercentage() { + testUntilPercentage(0.1); + tearDown(); + + setUp(); + testUntilPercentage(0.2); + tearDown(); + + setUp(); + testUntilPercentage(0.3); + tearDown(); + + setUp(); + testUntilPercentage(0.4); + tearDown(); + + setUp(); + testUntilPercentage(0.6); + tearDown(); + + setUp(); + testUntilPercentage(0.7); + tearDown(); + + setUp(); + testUntilPercentage(0.8); + tearDown(); + + setUp(); + testUntilPercentage(0.9); + // Auto Teardown + } + + @Test + void testStatusFills() { + ledSubsystem.setStatusColor(false); + Timer.delay(0.1); + checkForColorInAll(ledSubsystem, Color.kRed, "Color Should be red when status is false"); + tearDown(); + + setUp(); + ledSubsystem.setStatusColor(true); + Timer.delay(0.1); + checkForColorInAll(ledSubsystem, Color.kGreen, "Color should be green when status is true"); + } +} diff --git a/src/test/java/subsystem_tests/led_tests/utils/LEDTestUtils.java b/src/test/java/subsystem_tests/led_tests/utils/LEDTestUtils.java new file mode 100644 index 0000000..ba3593b --- /dev/null +++ b/src/test/java/subsystem_tests/led_tests/utils/LEDTestUtils.java @@ -0,0 +1,44 @@ +package subsystem_tests.led_tests.utils; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.util.Color; +import frc.robot.commands.led_commands.LEDLoadingWaitCommand; +import frc.robot.subsystems.LEDSubsystem; + +public class LEDTestUtils { + public static void checkForColorInAll( + LEDSubsystem ledSubsystem, Color colorItShouldBe, int untilIndex, String message) { + for (int i = 0; i < untilIndex; i++) { + assertEquals(colorItShouldBe, getColorAtIndex(ledSubsystem, i), message + " at index: " + i); + } + } + + public static void checkForColorInAll( + LEDSubsystem ledSubsystem, Color colorItShouldBe, String message) { + checkForColorInAll(ledSubsystem, colorItShouldBe, ledSubsystem.getLedCount(), message); + } + + /** + * Auto Calculates the end time + * + * @param ledSubsystem + * @param startTime + * @param waitTime + */ + public static void testAtTime(LEDSubsystem ledSubsystem, double startTime, double waitTime) { + double endTime = Timer.getFPGATimestamp(); + int untilIndex = (int) (ledSubsystem.getLedCount() * ((endTime - startTime) / waitTime)); + Timer.delay(0.1); // let led loop do its thing + checkForColorInAll( + ledSubsystem, + LEDLoadingWaitCommand.DEFAULT_COLOR, + untilIndex, + "Color should be default color until %s".formatted(untilIndex)); + } + + public static Color getColorAtIndex(LEDSubsystem ledSubsystem, int index) { + return ledSubsystem.getStrip().getBuffer().getLED(index); + } +} diff --git a/src/test/java/ColorSensorV3WrapperTests.java b/src/test/java/wrapper_tests/ColorSensorV3WrapperTests.java similarity index 85% rename from src/test/java/ColorSensorV3WrapperTests.java rename to src/test/java/wrapper_tests/ColorSensorV3WrapperTests.java index 9053ff8..4867421 100644 --- a/src/test/java/ColorSensorV3WrapperTests.java +++ b/src/test/java/wrapper_tests/ColorSensorV3WrapperTests.java @@ -1,3 +1,5 @@ +package wrapper_tests; + import static org.junit.jupiter.api.Assertions.assertEquals; import edu.wpi.first.hal.HAL; @@ -23,6 +25,12 @@ public void tearDown() { @Test void testColorSensorV3Wrapping() { + assertEquals( + colorSensor, ColorSensorV3Wrapped.getColorSensor(), "ColorSensorV3 should be wrapped"); + } + + @Test + void testColorSensorV3ValueChanging() { ColorSensorV3Wrapped.setRGBD(2500, 300, 100, 900); assertEquals(2500, colorSensor.getRed()); assertEquals(300, colorSensor.getGreen()); diff --git a/src/test/java/wrapper_tests/SparkMAXWrapperTest.java b/src/test/java/wrapper_tests/SparkMAXWrapperTest.java new file mode 100644 index 0000000..71132f4 --- /dev/null +++ b/src/test/java/wrapper_tests/SparkMAXWrapperTest.java @@ -0,0 +1,48 @@ +package wrapper_tests; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.hal.HAL; +import frc.utils.sim_utils.CANSparkMAXWrapped; +import frc.utils.sim_utils.SparkMAXSimAddon; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +class SparkMAXWrapperTest { + @BeforeEach + public void setUp() { + HAL.initialize(500, 0); // init HAL just in case + } + + @Test + void testSparkMAXWrapper() { + for (int i = 0; i < 50; i++) { + CANSparkMAXWrapped sparkMAX = + new CANSparkMAXWrapped(i, CANSparkMAXWrapped.MotorType.kBrushless); + assertEquals(sparkMAX, SparkMAXSimAddon.getSparkMAX(i)); + sparkMAX.close(); + } + } + + @Test + void testSparkMAXisClosed() { + CANSparkMAXWrapped sparkMAX = + new CANSparkMAXWrapped(1, CANSparkMAXWrapped.MotorType.kBrushless); + sparkMAX.close(); + assertEquals(true, sparkMAX.isThisClosed()); + } + + @Test + void testSparkMAXClear() { + CANSparkMAXWrapped sparkMAX = + new CANSparkMAXWrapped(1, CANSparkMAXWrapped.MotorType.kBrushless); + SparkMAXSimAddon.resetData(); + try { + SparkMAXSimAddon.getSparkMAX(1); + assert false; // should not reach here + sparkMAX.close(); // just so the linter doesn't complain + } catch (IllegalArgumentException e) { + assertEquals("SparkMAX with deviceID 1 does not exist", e.getMessage()); + } + } +}