diff --git a/src/main/java/org/carlmontrobotics/lib199/safeMode/SafeCommand.java b/src/main/java/org/carlmontrobotics/lib199/safeMode/SafeCommand.java new file mode 100644 index 00000000..bc90ae01 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/safeMode/SafeCommand.java @@ -0,0 +1,42 @@ +package org.carlmontrobotics.lib199.safeMode; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.Subsystem; + +/** + * A command that only runs when safe-mode is enabled and returns {@code isFinished() = true} otherwise. + * + * Note that this does not block calls to {@link #end(boolean)} (If the command is scheduled when safe-mode is disabled, {@link #end(boolean)} will be called immediately}) + */ +public class SafeCommand extends FunctionalCommand { + + private final Command command; + + /** + * Creates a new SafeCommand + * @param command The command to run + */ + public SafeCommand(Command command) { + super( + () -> { if(SafeMode.isEnabled()) command.initialize(); }, + () -> { if(SafeMode.isEnabled()) command.execute(); }, + command::end, + () -> command.isFinished() || !SafeMode.isEnabled(), + command.getRequirements().toArray(Subsystem[]::new) + ); + CommandScheduler.getInstance().registerComposedCommands(this.command = command); + } + + @Override + public boolean runsWhenDisabled() { + return command.runsWhenDisabled(); + } + + @Override + public InterruptionBehavior getInterruptionBehavior() { + return command.getInterruptionBehavior(); + } + +} diff --git a/src/main/java/org/carlmontrobotics/lib199/safeMode/SafeExecuteBlockingCommand.java b/src/main/java/org/carlmontrobotics/lib199/safeMode/SafeExecuteBlockingCommand.java new file mode 100644 index 00000000..48e72fb9 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/safeMode/SafeExecuteBlockingCommand.java @@ -0,0 +1,39 @@ +package org.carlmontrobotics.lib199.safeMode; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.Subsystem; + +/** + * A command that only runs its {@link #execute()} method when safe-mode is enabled, and continues running as long as the underlying command is not finished. + * + * Keep in mind that this does not block calls to {@link #initialize()} or {@link #end(boolean)}, so it is not appropriate for wrapping commands such as {@link edu.wpi.first.wpilibj2.command.InstantCommand}. + * It is intended for cases where the command should keep running such as a default command where {@link #isFinished()} must always return {@code false} + */ +public class SafeExecuteBlockingCommand extends FunctionalCommand { + + private final Command command; + + public SafeExecuteBlockingCommand(Command command) { + super( + () -> command.initialize(), + () -> { if(SafeMode.isEnabled()) command.execute(); }, + command::end, + command::isFinished, + command.getRequirements().toArray(Subsystem[]::new) + ); + CommandScheduler.getInstance().registerComposedCommands(this.command = command); + } + + @Override + public boolean runsWhenDisabled() { + return command.runsWhenDisabled(); + } + + @Override + public InterruptionBehavior getInterruptionBehavior() { + return command.getInterruptionBehavior(); + } + +} diff --git a/src/main/java/org/carlmontrobotics/lib199/safeMode/SafeJoystick.java b/src/main/java/org/carlmontrobotics/lib199/safeMode/SafeJoystick.java new file mode 100644 index 00000000..de1f1fc6 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/safeMode/SafeJoystick.java @@ -0,0 +1,119 @@ +package org.carlmontrobotics.lib199.safeMode; + +import java.util.Map; +import java.util.Set; + +import edu.wpi.first.wpilibj.GenericHID; + +/** + * A wrapper for a {@link GenericHID} that implements safe-mode features. + * + * This class is designed for internal use via {@link org.carlmontrobotics.lib199.Mocks}. + * As such it provides re-implementations for {@link GenericHID} methods without extending {@link GenericHID} + * to allow for the extension of subclasses such as {@link edu.wpi.first.wpilibj.Joystick}, {@link edu.wpi.first.wpilibj.PS4Controller}, + * {@link edu.wpi.first.wpilibj.XboxController}, etc. To wrap a joystick, call {@link SafeMode#makeSafe(GenericHID)}. + */ +public class SafeJoystick { + + /** + * The unsafe joystick that this class wraps. + */ + public final GenericHID unsafeJoystick; + + private final Set safeDisabledButtons; + private final Set safeDisabledAxes; + private final Map safeScaledAxes; + private final Map> safeDisabledPOV; + + /** + * Creates a new SafeJoystick + * + * @param unsafeJoystick The unsafe joystick to wrap + * @param safeDisabledButtons The buttons that should be disabled in safe-mode + * @param safeDisabledAxes The axes that should be disabled in safe-mode + * @param safeScaledAxes The axes that should be scaled in safe-mode (key: axis, value: scale factor) + * @param safeDisabledPOV The POVs that should be disabled in safe-mode (key: POV, value: set of disabled values) + */ + public SafeJoystick(GenericHID unsafeJoystick, Set safeDisabledButtons, Set safeDisabledAxes, Map safeScaledAxes, Map> safeDisabledPOV) { + this.unsafeJoystick = unsafeJoystick; + this.safeDisabledButtons = safeDisabledButtons; + this.safeDisabledAxes = safeDisabledAxes; + this.safeScaledAxes = safeScaledAxes; + this.safeDisabledPOV = safeDisabledPOV; + } + + // All other methods always fall through to these five + + /** + * Safe version of {@link GenericHID#getRawButton(int)}. + * + * @param button The button to read + * @return The state of the button + */ + public boolean getRawButton(int button) { + if (SafeMode.isEnabled() && safeDisabledButtons.contains(button)) { + return false; + } else { + return unsafeJoystick.getRawButton(button); + } + } + + /** + * Safe version of {@link GenericHID#getRawButtonPressed(int)}. + * + * @param button The button to read + * @return Whether the button was pressed since the last check + */ + public boolean getRawButtonPressed(int button) { + if (SafeMode.isEnabled() && safeDisabledButtons.contains(button)) { + return false; + } else { + return unsafeJoystick.getRawButtonPressed(button); + } + } + + /** + * Safe version of {@link GenericHID#getRawButtonReleased(int)}. + * + * @param button The button to read + * @return Whether the button was released since the last check + */ + public boolean getRawButtonReleased(int button) { + if (SafeMode.isEnabled() && safeDisabledButtons.contains(button)) { + return false; + } else { + return unsafeJoystick.getRawButtonReleased(button); + } + } + + /** + * Safe version of {@link GenericHID#getRawAxis(int)}. + * + * @param axis The axis to read + * @return The value of the axis + */ + public double getRawAxis(int axis) { + if (SafeMode.isEnabled() && safeDisabledAxes.contains(axis)) { + return 0; + } else if (SafeMode.isEnabled() && safeScaledAxes.containsKey(axis)) { + return unsafeJoystick.getRawAxis(axis) * safeScaledAxes.get(axis); + } else { + return unsafeJoystick.getRawAxis(axis); + } + } + + /** + * Safe version of {@link GenericHID#getPOV(int)}. + * + * @param pov The POV to read + * @return The value of the POV + */ + public double getPOV(int pov) { + if (SafeMode.isEnabled() && safeDisabledPOV.containsKey(pov) && safeDisabledPOV.get(pov).contains(unsafeJoystick.getPOV(pov))) { + return -1; + } else { + return unsafeJoystick.getPOV(pov); + } + } + +} diff --git a/src/main/java/org/carlmontrobotics/lib199/safeMode/SafeMode.java b/src/main/java/org/carlmontrobotics/lib199/safeMode/SafeMode.java new file mode 100644 index 00000000..71e678b0 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/safeMode/SafeMode.java @@ -0,0 +1,358 @@ +package org.carlmontrobotics.lib199.safeMode; + +import java.util.Collections; +import java.util.HashSet; +import java.util.Map; +import java.util.Set; +import java.util.concurrent.ConcurrentHashMap; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import java.util.function.IntSupplier; +import java.util.function.LongSupplier; +import java.util.function.Supplier; + +import org.carlmontrobotics.lib199.Lib199Subsystem; +import org.carlmontrobotics.lib199.Mocks; + +import edu.wpi.first.networktables.BooleanEntry; +import edu.wpi.first.networktables.BooleanSubscriber; +import edu.wpi.first.networktables.BooleanTopic; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** + * A class that keeps track of safe-mode state and provides functions to access common safe-mode features + * + * To the best of my knowledge, all safe-mode features are thread-safe + */ +public class SafeMode { + + //#region Basic Safe Mode + + private static BooleanEntry safeModeStatus; + + // NetworkTables doesn't like it when we use the same subscriber for isEnabled() and updateCallbacks() + // My guess is that readQueueValues() interferes with the get() call + private static BooleanSubscriber enabledListener; + + static { + SmartDashboard.putBoolean("Safe Mode", false); + BooleanTopic safeModeTopic = new BooleanTopic(SmartDashboard.getEntry("Safe Mode").getTopic()); + safeModeStatus = safeModeTopic.getEntry(false); + enabledListener = safeModeTopic.subscribe(false); + + // Call updateCallbacks synchronously with the CommandScheduler + Lib199Subsystem.registerPeriodic(SafeMode::updateCallbacks); + } + + /** + * Enables safe-mode + */ + public static void enable() { + safeModeStatus.set(true); + } + + /** + * Disables safe-mode + */ + public static void disable() { + safeModeStatus.set(false); + } + + /** + * @return Whether safe-mode is enabled + */ + public static boolean isEnabled() { + return safeModeStatus.get(); + } + + //#endregion + + //#region Callbacks + + private static final Set onSafeModeEnabled = createEmptyThreadSafeSet(); + private static final Set onSafeModeDisabled = createEmptyThreadSafeSet(); + + // Guaranteed to be thread-safe + // Not guaranteed to have non-extraneous call + /** + * Registers a callback to be called when safe-mode is enabled. + * + * A couple notes about the state of the callback function: + * 1. This function is thread-safe + * 2. The callback function will be called synchronously with the CommandScheduler (unless you call {@link #updateCallbacks()} from another thread) + * 3. The callback function may be called multiple times in a row, even if the safe-mode state has not changed + * 4. The callback function will not be called if safe-mode is not enabled + * 5. The callback function will be called if safe-mode is enabled unless safe-mode is re-disabled before a periodic update + * 6. The callback function will not necessarily be invoked based on the current state of safe-mode + * + * @param runnable The function to run + */ + public static void onEnabled(Runnable runnable) { + onSafeModeEnabled.add(runnable); + } + + /** + * Registers a callback to be called when safe-mode is disabled. + * + * A couple notes about the state of the callback function: + * 1. This function is thread-safe + * 2. The callback function will be called synchronously with the CommandScheduler (unless you call {@link #updateCallbacks()} from another thread) + * 3. The callback function may be called multiple times in a row, even if the safe-mode state has not changed + * 4. The callback function will not be called if safe-mode is not disabled + * 5. The callback function will be called if safe-mode is disabled unless safe-mode is re-enabled before a periodic update + * 6. The callback function will not necessarily be invoked based on the current state of safe-mode + * + * @param runnable The function to run + */ + public static void onDisabled(Runnable runnable) { + onSafeModeDisabled.add(runnable); + } + + /** + * Calls the callbacks for safe-mode state changes if necessary. + * It should not be necessary to call this function manually as it is automatically invoked as part of the robot's periodic loop via {@link CommandScheduler}. + * + * While this function is thread-safe, if you call it from another thread, the callbacks may not be called synchronously with the {@link CommandScheduler}. + */ + public static void updateCallbacks() { + boolean stateChanged = enabledListener.readQueueValues().length != 0; + if(stateChanged) { + if (isEnabled()) { + onSafeModeEnabled.forEach(Runnable::run); + } else { + onSafeModeDisabled.forEach(Runnable::run); + } + } + } + + //#endregion + + //#region Safe Constants + + /** + * Creates a {@link Supplier} that returns a constant value when safe-mode is disabled and a different constant value when safe-mode is enabled. + * + * @param The type of the constant values + * @param normalValue The value to return when safe-mode is disabled + * @param safeValue The value to return when safe-mode is enabled + * @return A supplier which selects the appropriate value when called + */ + public static Supplier constant(T normalValue, T safeValue) { + return () -> isEnabled() ? safeValue : normalValue; + } + + /** + * Creates a {@link BooleanSupplier} that returns a constant value when safe-mode is disabled and a different constant value when safe-mode is enabled. + * + * @param normalValue The value to return when safe-mode is disabled + * @param safeValue The value to return when safe-mode is enabled + * @return A supplier which selects the appropriate value when called + */ + public static BooleanSupplier constant(boolean normalValue, boolean safeValue) { + return () -> isEnabled() ? safeValue : normalValue; + } + + /** + * Creates a {@link DoubleSupplier} that returns a constant value when safe-mode is disabled and a different constant value when safe-mode is enabled. + * + * @param normalValue The value to return when safe-mode is disabled + * @param safeValue The value to return when safe-mode is enabled + * @return A supplier which selects the appropriate value when called + */ + public static DoubleSupplier constant(double normalValue, double safeValue) { + return () -> isEnabled() ? safeValue : normalValue; + } + + /** + * Creates an {@link IntSupplier} that returns a constant value when safe-mode is disabled and a different constant value when safe-mode is enabled. + * + * @param normalValue The value to return when safe-mode is disabled + * @param safeValue The value to return when safe-mode is enabled + * @return A supplier which selects the appropriate value when called + */ + public static IntSupplier constant(int normalValue, int safeValue) { + return () -> isEnabled() ? safeValue : normalValue; + } + + /** + * Creates a {@link LongSupplier} that returns a constant value when safe-mode is disabled and a different constant value when safe-mode is enabled. + * + * @param normalValue The value to return when safe-mode is disabled + * @param safeValue The value to return when safe-mode is enabled + * @return A supplier which selects the appropriate value when called + */ + public static LongSupplier constant(long normalValue, long safeValue) { + return () -> isEnabled() ? safeValue : normalValue; + } + + //#endregion + + //#region Safe Joystick + + // lib199 uses asynchronous code in a few places, so these will all be thread-safe + private static final Map> safeDisabledButtons = new ConcurrentHashMap<>(); + private static final Map> safeDisabledAxes = new ConcurrentHashMap<>(); + private static final Map> safeScaledAxes = new ConcurrentHashMap<>(); + private static final Map>> safeDisabledPOVs = new ConcurrentHashMap<>(); + + /** + * Creates a {@link GenericHID} which alters its outputs while safe-mode is enabled bassed on an underlying {@link GenericHID} implementation. + * + * @param The type of the underlying {@link GenericHID} + * @param joystick The underlying {@link GenericHID} implementation + * @return A {@link GenericHID} of the same type and based on the given joystick which alters its outputs while safe-mode is enabled + */ + @SuppressWarnings("unchecked") + public static T makeSafe(T joystick) { + safeDisabledAxes.putIfAbsent(joystick.getPort(), createEmptyThreadSafeSet()); + safeDisabledButtons.putIfAbsent(joystick.getPort(), createEmptyThreadSafeSet()); + safeScaledAxes.putIfAbsent(joystick.getPort(), new ConcurrentHashMap<>()); + safeDisabledPOVs.putIfAbsent(joystick.getPort(), new ConcurrentHashMap<>()); + + int port = joystick.getPort(); + return Mocks.createMock( + (Class) joystick.getClass(), + new SafeJoystick( + joystick, + safeDisabledButtons.get(port), + safeDisabledAxes.get(port), + safeScaledAxes.get(port), + safeDisabledPOVs.get(port) + ) + ); + } + + /** + * Disables a button on a joystick while safe-mode is enabled. + * + * @param joystickPort The port of the joystick + * @param button The button to disable + */ + public static void disableButton(int joystickPort, int button) { + safeDisabledButtons.putIfAbsent(joystickPort, createEmptyThreadSafeSet()); + safeDisabledButtons.get(joystickPort).add(button); + } + + /** + * Disables a button on a joystick while safe-mode is enabled. + * + * @param joystick The joystick to disable the button on + * @param button The button to disable + */ + public static void disableButton(GenericHID joystick, int button) { + disableButton(joystick.getPort(), button); + } + + /** + * Disables an axis on a joystick while safe-mode is enabled. + * + * @param joystickPort The port of the joystick + * @param axis The axis to disable + */ + public static void disableAxis(int joystickPort, int axis) { + safeDisabledAxes.putIfAbsent(joystickPort, createEmptyThreadSafeSet()); + safeDisabledAxes.get(joystickPort).add(axis); + } + + /** + * Disables an axis on a joystick while safe-mode is enabled. + * + * @param joystick The joystick to disable the axis on + * @param axis The axis to disable + */ + public static void disableAxis(GenericHID joystick, int axis) { + disableAxis(joystick.getPort(), axis); + } + + /** + * Scales an axis on a joystick while safe-mode is enabled. + * + * @param joystickPort The port of the joystick + * @param axis The axis to scale + * @param factor The factor to scale the axis by + */ + public static void scaleAxis(int joystickPort, int axis, double factor) { + safeScaledAxes.putIfAbsent(joystickPort, new ConcurrentHashMap<>()); + safeScaledAxes.get(joystickPort).put(axis, factor); + } + + /** + * Scales an axis on a joystick while safe-mode is enabled. + * + * @param joystick The joystick to scale the axis on + * @param axis The axis to scale + * @param factor The factor to scale the axis by + */ + public static void scaleAxis(GenericHID joystick, int axis, double factor) { + scaleAxis(joystick.getPort(), axis, factor); + } + + /** + * Disables a POV state (on POV 0) on a joystick while safe-mode is enabled. + * + * This is equivalent to calling {@code disablePOV(joystickPort, 0, angle)}. + * + * @param joystickPort The port of the joystick + * @param angle The angle of the POV to disable + */ + public static void disablePOV(int joystickPort, int angle) { + disablePOV(joystickPort, 0, angle); + } + + /** + * Disables a POV state (on POV 0) on a joystick while safe-mode is enabled. + * + * This is equivalent to calling {@code disablePOV(joystick, 0, angle)}. + * + * @param joystick The joystick to disable the POV on + * @param angle The angle of the POV to disable + */ + public static void disablePOV(GenericHID joystick, int angle) { + disablePOV(joystick.getPort(), angle); + } + + /** + * Disables a POV state on a joystick while safe-mode is enabled. + * + * @param joystickPort The port of the joystick + * @param pov The POV on the joystick + * @param angle The angle of the POV to disable + */ + public static void disablePOV(int joystickPort, int pov, int angle) { + safeDisabledPOVs.putIfAbsent(joystickPort, new ConcurrentHashMap<>()); + safeDisabledPOVs.get(joystickPort).putIfAbsent(angle, createEmptyThreadSafeSet()); + safeDisabledPOVs.get(joystickPort).get(angle).add(angle); + } + + /** + * Disables a POV state on a joystick while safe-mode is enabled. + * + * @param joystick The joystick to disable the POV on + * @param pov The POV on the joystick + * @param angle The angle of the POV to disable + */ + public static void disablePOV(GenericHID joystick, int pov, int angle) { + disablePOV(joystick.getPort(), pov, angle); + } + + //#endregion + + /** + * Creates an empty thread-safe set. + * + * NOTE: Unlike with maps, there are a few different ways to make sets thread-safe + * I chose this method of creating synchronized sets based on + * https://stackoverflow.com/questions/6720396/different-types-of-thread-safe-sets-in-java, and https://docs.oracle.com/javase/tutorial/collections/implementations/set.html + * This method SHOULD NOT be used outside of safe-mode-related code! (hence why it's not public) + * Please make your own determination for other areas of the code rather than just copying this. + * + * @param The type of the set + * @return An empty thread-safe set of type {@code T} + */ + static Set createEmptyThreadSafeSet() { + return Collections.synchronizedSet(new HashSet<>()); + } + +} diff --git a/src/main/java/org/carlmontrobotics/lib199/safeMode/UnsafeCommand.java b/src/main/java/org/carlmontrobotics/lib199/safeMode/UnsafeCommand.java new file mode 100644 index 00000000..ce7b80a5 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/safeMode/UnsafeCommand.java @@ -0,0 +1,38 @@ +package org.carlmontrobotics.lib199.safeMode; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.Subsystem; + +/** + * A command that only runs when safe-mode is disabled and returns {@code isFinished() = true} otherwise. + * + * Note that this does not block calls to {@link #end(boolean)} (If the command is scheduled when safe-mode is enabled, {@link #end(boolean)} will be called immediately}) + */ +public class UnsafeCommand extends FunctionalCommand { + + private final Command command; + + public UnsafeCommand(Command command) { + super( + () -> { if(!SafeMode.isEnabled()) command.initialize(); }, + () -> { if(!SafeMode.isEnabled()) command.execute(); }, + command::end, + () -> command.isFinished() || SafeMode.isEnabled(), + command.getRequirements().toArray(Subsystem[]::new) + ); + CommandScheduler.getInstance().registerComposedCommands(this.command = command); + } + + @Override + public boolean runsWhenDisabled() { + return command.runsWhenDisabled(); + } + + @Override + public InterruptionBehavior getInterruptionBehavior() { + return command.getInterruptionBehavior(); + } + +} diff --git a/src/main/java/org/carlmontrobotics/lib199/safeMode/UnsafeExecuteBlockingCommand.java b/src/main/java/org/carlmontrobotics/lib199/safeMode/UnsafeExecuteBlockingCommand.java new file mode 100644 index 00000000..c9072898 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/safeMode/UnsafeExecuteBlockingCommand.java @@ -0,0 +1,39 @@ +package org.carlmontrobotics.lib199.safeMode; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.Subsystem; + +/** + * A command that only runs its {@link #execute()} method when safe-mode is disabled, and continues running as long as the underlying command is not finished. + * + * Keep in mind that this does not block calls to {@link #initialize()} or {@link #end(boolean)}, so it is not appropriate for wrapping commands such as {@link edu.wpi.first.wpilibj2.command.InstantCommand}. + * It is intended for cases where the command should keep running such as a default command where {@link #isFinished()} must always return {@code false} + */ +public class UnsafeExecuteBlockingCommand extends FunctionalCommand { + + private final Command command; + + public UnsafeExecuteBlockingCommand(Command command) { + super( + command::initialize, + () -> { if (!SafeMode.isEnabled()) command.execute(); }, + command::end, + command::isFinished, + command.getRequirements().toArray(Subsystem[]::new) + ); + CommandScheduler.getInstance().registerComposedCommands(this.command = command); + } + + @Override + public boolean runsWhenDisabled() { + return command.runsWhenDisabled(); + } + + @Override + public InterruptionBehavior getInterruptionBehavior() { + return command.getInterruptionBehavior(); + } + +} diff --git a/src/test/java/org/carlmontrobotics/lib199/safeMode/LoggingCommand.java b/src/test/java/org/carlmontrobotics/lib199/safeMode/LoggingCommand.java new file mode 100644 index 00000000..52996dff --- /dev/null +++ b/src/test/java/org/carlmontrobotics/lib199/safeMode/LoggingCommand.java @@ -0,0 +1,55 @@ +package org.carlmontrobotics.lib199.safeMode; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +/** + * A command that counts how many times it has been initialized and executed for the purposes of testing safe mode functionality. + * + * @see SafeModeCommandsTest + */ +public class LoggingCommand extends CommandBase { + + private int initializedCount = 0, executeCount = 0; + + @Override + public void initialize() { + initializedCount++; + } + + @Override + public void execute() { + executeCount++; + } + + /** + * Resets the initialized and execute counts to zero. + */ + public void reset() { + initializedCount = executeCount = 0; + } + + /** + * @return The number of times this command has been initialized. + */ + public int getInitializedCount() { + return initializedCount; + } + + /** + * @return The number of times this command has been executed. + */ + public int getExecuteCount() { + return executeCount; + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public boolean runsWhenDisabled() { + return true; + } + +} diff --git a/src/test/java/org/carlmontrobotics/lib199/safeMode/SafeJoystickTest.java b/src/test/java/org/carlmontrobotics/lib199/safeMode/SafeJoystickTest.java new file mode 100644 index 00000000..7483c553 --- /dev/null +++ b/src/test/java/org/carlmontrobotics/lib199/safeMode/SafeJoystickTest.java @@ -0,0 +1,132 @@ +package org.carlmontrobotics.lib199.safeMode; + +import static org.junit.Assert.*; + +import edu.wpi.first.wpilibj.GenericHID; + +public class SafeJoystickTest { + + public void testSafeJoystick() { + GenericHID normalJoystick = createDummyJoystick(0); + GenericHID unsafeJoystick1 = createDummyJoystick(1); + GenericHID unsafeJoystick2 = createDummyJoystick(2); + + GenericHID safeJoystick1 = SafeMode.makeSafe(unsafeJoystick1); + + SafeMode.disableButton(1, 0); + SafeMode.disableAxis(1, 0); + SafeMode.scaleAxis(1, 1, 0.5); + SafeMode.disablePOV(1, 0); + SafeMode.disablePOV(1, 1, 0); + + SafeMode.disableButton(2, 0); + SafeMode.disableAxis(2, 0); + SafeMode.scaleAxis(2, 1, 0.5); + SafeMode.disablePOV(2, 0); + SafeMode.disablePOV(2, 1, 0); + + GenericHID safeJoystick2 = SafeMode.makeSafe(unsafeJoystick2); + + SafeMode.disable(); + + assertTrue(normalJoystick.getRawButton(0)); + assertTrue(normalJoystick.getRawButtonPressed(0)); + assertTrue(normalJoystick.getRawButtonReleased(0)); + assertTrue(normalJoystick.getRawButton(1)); + assertTrue(normalJoystick.getRawButtonPressed(1)); + assertTrue(normalJoystick.getRawButtonReleased(1)); + assertEquals(1.0, normalJoystick.getRawAxis(0), 0.01); + assertEquals(1.0, normalJoystick.getRawAxis(1), 0.01); + assertEquals(1.0, normalJoystick.getRawAxis(2), 0.01); + assertEquals(0, normalJoystick.getPOV(0)); + assertEquals(90, normalJoystick.getPOV(1)); + + assertTrue(safeJoystick1.getRawButton(0)); + assertTrue(safeJoystick1.getRawButtonPressed(0)); + assertTrue(safeJoystick1.getRawButtonReleased(0)); + assertTrue(safeJoystick1.getRawButton(1)); + assertTrue(safeJoystick1.getRawButtonPressed(1)); + assertTrue(safeJoystick1.getRawButtonReleased(1)); + assertEquals(1.0, safeJoystick1.getRawAxis(0), 0.01); + assertEquals(1.0, safeJoystick1.getRawAxis(1), 0.01); + assertEquals(1.0, safeJoystick1.getRawAxis(2), 0.01); + assertEquals(0, safeJoystick1.getPOV(0)); + assertEquals(90, safeJoystick1.getPOV(1)); + + assertTrue(safeJoystick2.getRawButton(0)); + assertTrue(safeJoystick2.getRawButtonPressed(0)); + assertTrue(safeJoystick2.getRawButtonReleased(0)); + assertTrue(safeJoystick2.getRawButton(1)); + assertTrue(safeJoystick2.getRawButtonPressed(1)); + assertTrue(safeJoystick2.getRawButtonReleased(1)); + assertEquals(1.0, safeJoystick2.getRawAxis(0), 0.01); + assertEquals(1.0, safeJoystick2.getRawAxis(1), 0.01); + assertEquals(1.0, safeJoystick2.getRawAxis(2), 0.01); + assertEquals(0, safeJoystick2.getPOV(0)); + assertEquals(90, safeJoystick2.getPOV(1)); + + SafeMode.enable(); + + assertTrue(normalJoystick.getRawButton(0)); + assertTrue(normalJoystick.getRawButtonPressed(0)); + assertTrue(normalJoystick.getRawButtonReleased(0)); + assertTrue(normalJoystick.getRawButton(1)); + assertTrue(normalJoystick.getRawButtonPressed(1)); + assertTrue(normalJoystick.getRawButtonReleased(1)); + assertEquals(1.0, normalJoystick.getRawAxis(0), 0.01); + assertEquals(1.0, normalJoystick.getRawAxis(1), 0.01); + assertEquals(1.0, normalJoystick.getRawAxis(2), 0.01); + assertEquals(0, normalJoystick.getPOV(0)); + assertEquals(90, normalJoystick.getPOV(1)); + + assertFalse(safeJoystick1.getRawButton(0)); + assertFalse(safeJoystick1.getRawButtonPressed(0)); + assertFalse(safeJoystick1.getRawButtonReleased(0)); + assertTrue(safeJoystick1.getRawButton(1)); + assertTrue(safeJoystick1.getRawButtonPressed(1)); + assertTrue(safeJoystick1.getRawButtonReleased(1)); + assertEquals(0.5, safeJoystick1.getRawAxis(0), 0.01); + assertEquals(0.5, safeJoystick1.getRawAxis(1), 0.01); + assertEquals(1.0, safeJoystick1.getRawAxis(2), 0.01); + assertEquals(-1, safeJoystick1.getPOV(0)); + assertEquals(90, safeJoystick1.getPOV(1)); + + assertFalse(safeJoystick2.getRawButton(0)); + assertFalse(safeJoystick2.getRawButtonPressed(0)); + assertFalse(safeJoystick2.getRawButtonReleased(0)); + assertTrue(safeJoystick2.getRawButton(1)); + assertTrue(safeJoystick2.getRawButtonPressed(1)); + assertTrue(safeJoystick2.getRawButtonReleased(1)); + assertEquals(0.0, safeJoystick2.getRawAxis(0), 0.01); + assertEquals(0.5, safeJoystick2.getRawAxis(1), 0.01); + assertEquals(1.0, safeJoystick2.getRawAxis(2), 0.01); + assertEquals(-1, safeJoystick2.getPOV(0)); + assertEquals(90, safeJoystick2.getPOV(1)); + } + + private static GenericHID createDummyJoystick(int port) { + return new GenericHID(port) { + public boolean getRawButton(int button) { + return true; + } + + @Override + public boolean getRawButtonPressed(int button) { + return true; + } + + @Override + public boolean getRawButtonReleased(int button) { + return true; + } + + public double getRawAxis(int axis) { + return 1.0; + } + + public int getPOV(int pov) { + return pov == 0 ? 0 : 90; + } + }; + } +} diff --git a/src/test/java/org/carlmontrobotics/lib199/safeMode/SafeModeCommandsTest.java b/src/test/java/org/carlmontrobotics/lib199/safeMode/SafeModeCommandsTest.java new file mode 100644 index 00000000..e465c589 --- /dev/null +++ b/src/test/java/org/carlmontrobotics/lib199/safeMode/SafeModeCommandsTest.java @@ -0,0 +1,65 @@ +package org.carlmontrobotics.lib199.safeMode; + +import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertFalse; +import static org.junit.Assert.assertTrue; + +import java.util.function.Function; + +import org.junit.Test; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +public class SafeModeCommandsTest { + + @Test + public void testSafeCommand() { + testCommand(SafeCommand::new, true, false); + } + + @Test + public void testSafeBlockingCommand() { + testCommand(SafeExecuteBlockingCommand::new, true, true); + } + + @Test + public void testUnsafeCommand() { + testCommand(UnsafeCommand::new, false, false); + } + + @Test + public void testUnsafeExecuteBlockingCommand() { + testCommand(UnsafeExecuteBlockingCommand::new, false, true); + } + + public void testCommand(Function constructor, boolean isSafe, boolean staysEnabled) { + LoggingCommand loggingCommand = new LoggingCommand(); + Command command = constructor.apply(loggingCommand); + + loggingCommand.reset(); + + SafeMode.enable(); + command.schedule(); + CommandScheduler.getInstance().run(); + if (isSafe || staysEnabled) + assertTrue(command.isScheduled()); + else + assertFalse(command.isScheduled()); + if (!staysEnabled) + assertEquals(isSafe ? 1 : 0, loggingCommand.getInitializedCount()); + assertEquals(isSafe ? 1 : 0, loggingCommand.getExecuteCount()); + + SafeMode.disable(); + command.schedule(); + CommandScheduler.getInstance().run(); + if (!isSafe || staysEnabled) + assertTrue(command.isScheduled()); + else + assertFalse(command.isScheduled()); + if (!staysEnabled) + assertEquals(1, loggingCommand.getInitializedCount()); + assertEquals(1, loggingCommand.getExecuteCount()); + } + +} diff --git a/src/test/java/org/carlmontrobotics/lib199/safeMode/SafeModeTest.java b/src/test/java/org/carlmontrobotics/lib199/safeMode/SafeModeTest.java new file mode 100644 index 00000000..4e993564 --- /dev/null +++ b/src/test/java/org/carlmontrobotics/lib199/safeMode/SafeModeTest.java @@ -0,0 +1,85 @@ +package org.carlmontrobotics.lib199.safeMode; + +import static org.junit.Assert.*; + +import java.util.concurrent.atomic.AtomicInteger; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import java.util.function.IntSupplier; +import java.util.function.LongSupplier; +import java.util.function.Supplier; + +import org.junit.Test; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +public class SafeModeTest { + + // Test SafeMode.java using JUnit + + @Test + public void testSafeModeEnableDisable() { + SafeMode.enable(); + assertTrue(SafeMode.isEnabled()); + assertTrue(SmartDashboard.getBoolean("Safe Mode", false)); + + SafeMode.disable(); + assertFalse(SafeMode.isEnabled()); + assertFalse(SmartDashboard.getBoolean("Safe Mode", true)); + + SmartDashboard.putBoolean("Safe Mode", false); + assertFalse(SafeMode.isEnabled()); + assertFalse(SmartDashboard.getBoolean("Safe Mode", true)); + + SmartDashboard.putBoolean("Safe Mode", true); + assertTrue(SafeMode.isEnabled()); + assertTrue(SmartDashboard.getBoolean("Safe Mode", false)); + } + + @Test + public void testCallbacks() { + AtomicInteger enabledCounter = new AtomicInteger(0); + AtomicInteger disabledCounter = new AtomicInteger(0); + + SafeMode.disable(); // The callbacks only get called when the state changes, so we need to start in a known state + + SafeMode.onEnabled(() -> enabledCounter.incrementAndGet()); + SafeMode.onDisabled(() -> disabledCounter.incrementAndGet()); + + SafeMode.enable(); + CommandScheduler.getInstance().run(); + assertEquals(1, enabledCounter.get()); + assertEquals(0, disabledCounter.get()); + + SafeMode.disable(); + CommandScheduler.getInstance().run(); + assertEquals(1, enabledCounter.get()); + assertEquals(1, disabledCounter.get()); + } + + @Test + public void testSafeConstants() { + Supplier safeString = SafeMode.constant("normal", "safe"); + BooleanSupplier safeBoolean = SafeMode.constant(false, true); + DoubleSupplier safeDouble = SafeMode.constant(1.0, 2.0); + IntSupplier safeInt = SafeMode.constant(1, 2); + LongSupplier safeLong = SafeMode.constant(1L, 2L); + + SafeMode.disable(); + assertEquals("normal", safeString.get()); + assertEquals(false, safeBoolean.getAsBoolean()); + assertEquals(1.0, safeDouble.getAsDouble(), 0.0); + assertEquals(1, safeInt.getAsInt()); + assertEquals(1L, safeLong.getAsLong()); + + SafeMode.enable(); + assertEquals("safe", safeString.get()); + assertEquals(true, safeBoolean.getAsBoolean()); + assertEquals(2.0, safeDouble.getAsDouble(), 0.0); + assertEquals(2, safeInt.getAsInt()); + assertEquals(2L, safeLong.getAsLong()); + } + + +}