From ddbb99d1e8f2884d5ed2b73e202a9a9f8957022f Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Sun, 9 Jul 2023 19:23:54 -0700 Subject: [PATCH] better safe-mode commands --- .../lib199/safeMode/EndBlockingCommand.java | 39 +++++++++++++++++++ .../lib199/safeMode/SafeCommand.java | 34 +++++++--------- .../safeMode/SafeExecuteBlockingCommand.java | 26 +++---------- .../lib199/safeMode/UnsafeCommand.java | 35 +++++++---------- .../UnsafeExecuteBlockingCommand.java | 26 +++---------- .../safeMode/EndBlockingCommandTest.java | 21 ++++++++++ .../lib199/safeMode/LoggingCommand.java | 18 +++++++-- .../lib199/safeMode/SafeModeCommandsTest.java | 21 +++++++--- 8 files changed, 128 insertions(+), 92 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/lib199/safeMode/EndBlockingCommand.java create mode 100644 src/test/java/org/carlmontrobotics/lib199/safeMode/EndBlockingCommandTest.java diff --git a/src/main/java/org/carlmontrobotics/lib199/safeMode/EndBlockingCommand.java b/src/main/java/org/carlmontrobotics/lib199/safeMode/EndBlockingCommand.java new file mode 100644 index 00000000..3ff8697c --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/safeMode/EndBlockingCommand.java @@ -0,0 +1,39 @@ +package org.carlmontrobotics.lib199.safeMode; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.WrapperCommand; + +/** + * A command who's {@link #end(boolean)} method can only be called if its {@link #initialize()} method has been called first. + */ +public class EndBlockingCommand extends WrapperCommand { + + private boolean initialized = false; + + /** + * Creates a new EndBlockingCommand + * @param command The command to run + */ + public EndBlockingCommand(Command command) { + super(command); + } + + @Override + public void initialize() { + synchronized(this) { + initialized = true; + } + super.initialize(); + } + + @Override + public void end(boolean interrupted) { + synchronized(this) { + if(initialized) { + super.end(interrupted); + initialized = false; + } + } + } + +} diff --git a/src/main/java/org/carlmontrobotics/lib199/safeMode/SafeCommand.java b/src/main/java/org/carlmontrobotics/lib199/safeMode/SafeCommand.java index bc90ae01..6ac2c23f 100644 --- a/src/main/java/org/carlmontrobotics/lib199/safeMode/SafeCommand.java +++ b/src/main/java/org/carlmontrobotics/lib199/safeMode/SafeCommand.java @@ -1,42 +1,34 @@ 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; +import edu.wpi.first.wpilibj2.command.WrapperCommand; /** - * 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}) + * A command that only runs when safe-mode is enabled and returns {@code isFinished() = true} otherwise. If safe-mode is disabled, {@code end(true)} will be called. */ -public class SafeCommand extends FunctionalCommand { - - private final Command command; +public class SafeCommand extends WrapperCommand { /** * 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); + super(new EndBlockingCommand(command)); } @Override - public boolean runsWhenDisabled() { - return command.runsWhenDisabled(); + public void initialize() { + if(SafeMode.isEnabled()) super.initialize(); } @Override - public InterruptionBehavior getInterruptionBehavior() { - return command.getInterruptionBehavior(); + public void execute() { + if(SafeMode.isEnabled()) super.execute(); } + @Override + public boolean isFinished() { + if(!SafeMode.isEnabled()) end(true); + return super.isFinished() || !SafeMode.isEnabled(); + } } diff --git a/src/main/java/org/carlmontrobotics/lib199/safeMode/SafeExecuteBlockingCommand.java b/src/main/java/org/carlmontrobotics/lib199/safeMode/SafeExecuteBlockingCommand.java index 48e72fb9..50f4c2ba 100644 --- a/src/main/java/org/carlmontrobotics/lib199/safeMode/SafeExecuteBlockingCommand.java +++ b/src/main/java/org/carlmontrobotics/lib199/safeMode/SafeExecuteBlockingCommand.java @@ -1,9 +1,7 @@ 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; +import edu.wpi.first.wpilibj2.command.WrapperCommand; /** * 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. @@ -11,29 +9,15 @@ * 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 class SafeExecuteBlockingCommand extends WrapperCommand { 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(); + super(command); } @Override - public InterruptionBehavior getInterruptionBehavior() { - return command.getInterruptionBehavior(); + public void execute() { + if(SafeMode.isEnabled()) super.execute(); } } diff --git a/src/main/java/org/carlmontrobotics/lib199/safeMode/UnsafeCommand.java b/src/main/java/org/carlmontrobotics/lib199/safeMode/UnsafeCommand.java index ce7b80a5..62b672e1 100644 --- a/src/main/java/org/carlmontrobotics/lib199/safeMode/UnsafeCommand.java +++ b/src/main/java/org/carlmontrobotics/lib199/safeMode/UnsafeCommand.java @@ -1,38 +1,31 @@ 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; +import edu.wpi.first.wpilibj2.command.WrapperCommand; /** - * 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}) + * A command that only runs when safe-mode is disabled and returns {@code isFinished() = true} otherwise. If safe-mode is enabled, {@code end(true)} will be called. */ -public class UnsafeCommand extends FunctionalCommand { - - private final Command command; +public class UnsafeCommand extends WrapperCommand { 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); + super(new EndBlockingCommand(command)); + } + + @Override + public void initialize() { + if(!SafeMode.isEnabled()) super.initialize(); } @Override - public boolean runsWhenDisabled() { - return command.runsWhenDisabled(); + public void execute() { + if(!SafeMode.isEnabled()) super.execute(); } @Override - public InterruptionBehavior getInterruptionBehavior() { - return command.getInterruptionBehavior(); + public boolean isFinished() { + if(SafeMode.isEnabled()) end(true); + return SafeMode.isEnabled() || super.isFinished(); } } diff --git a/src/main/java/org/carlmontrobotics/lib199/safeMode/UnsafeExecuteBlockingCommand.java b/src/main/java/org/carlmontrobotics/lib199/safeMode/UnsafeExecuteBlockingCommand.java index c9072898..60b21b74 100644 --- a/src/main/java/org/carlmontrobotics/lib199/safeMode/UnsafeExecuteBlockingCommand.java +++ b/src/main/java/org/carlmontrobotics/lib199/safeMode/UnsafeExecuteBlockingCommand.java @@ -1,9 +1,7 @@ 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; +import edu.wpi.first.wpilibj2.command.WrapperCommand; /** * 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. @@ -11,29 +9,15 @@ * 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 class UnsafeExecuteBlockingCommand extends WrapperCommand { 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(); + super(command); } @Override - public InterruptionBehavior getInterruptionBehavior() { - return command.getInterruptionBehavior(); + public void execute() { + if (!SafeMode.isEnabled()) super.execute(); } } diff --git a/src/test/java/org/carlmontrobotics/lib199/safeMode/EndBlockingCommandTest.java b/src/test/java/org/carlmontrobotics/lib199/safeMode/EndBlockingCommandTest.java new file mode 100644 index 00000000..a0ec3df9 --- /dev/null +++ b/src/test/java/org/carlmontrobotics/lib199/safeMode/EndBlockingCommandTest.java @@ -0,0 +1,21 @@ +package org.carlmontrobotics.lib199.safeMode; + +import static org.junit.Assert.assertEquals; + +import org.junit.Test; + +public class EndBlockingCommandTest { + + @Test + public void testEndBlockingCommand() { + LoggingCommand loggingCommand = new LoggingCommand(); + EndBlockingCommand command = new EndBlockingCommand(loggingCommand); + loggingCommand.reset(); + command.initialize(); + command.end(false); + assertEquals(1, loggingCommand.getEndCount()); + command.end(false); + assertEquals(1, loggingCommand.getEndCount()); + } + +} diff --git a/src/test/java/org/carlmontrobotics/lib199/safeMode/LoggingCommand.java b/src/test/java/org/carlmontrobotics/lib199/safeMode/LoggingCommand.java index 52996dff..7eec7241 100644 --- a/src/test/java/org/carlmontrobotics/lib199/safeMode/LoggingCommand.java +++ b/src/test/java/org/carlmontrobotics/lib199/safeMode/LoggingCommand.java @@ -3,13 +3,13 @@ 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. + * A command that counts how many times it has been initialized, executed, and ended for the purposes of testing safe mode functionality. * * @see SafeModeCommandsTest */ public class LoggingCommand extends CommandBase { - private int initializedCount = 0, executeCount = 0; + private int initializedCount = 0, executeCount = 0, endCount = 0; @Override public void initialize() { @@ -21,11 +21,16 @@ public void execute() { executeCount++; } + @Override + public void end(boolean interrupted) { + endCount++; + } + /** * Resets the initialized and execute counts to zero. */ public void reset() { - initializedCount = executeCount = 0; + initializedCount = executeCount = endCount = 0; } /** @@ -42,6 +47,13 @@ public int getExecuteCount() { return executeCount; } + /** + * @return The number of times this command has been ended. + */ + public int getEndCount() { + return endCount; + } + @Override public boolean isFinished() { return false; diff --git a/src/test/java/org/carlmontrobotics/lib199/safeMode/SafeModeCommandsTest.java b/src/test/java/org/carlmontrobotics/lib199/safeMode/SafeModeCommandsTest.java index e465c589..49a33b0f 100644 --- a/src/test/java/org/carlmontrobotics/lib199/safeMode/SafeModeCommandsTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/safeMode/SafeModeCommandsTest.java @@ -1,8 +1,6 @@ package org.carlmontrobotics.lib199.safeMode; -import static org.junit.Assert.assertEquals; -import static org.junit.Assert.assertFalse; -import static org.junit.Assert.assertTrue; +import static org.junit.Assert.*; import java.util.function.Function; @@ -46,8 +44,10 @@ public void testCommand(Function constructor, boolean isSafe, assertTrue(command.isScheduled()); else assertFalse(command.isScheduled()); - if (!staysEnabled) + if (!staysEnabled) { assertEquals(isSafe ? 1 : 0, loggingCommand.getInitializedCount()); + assertEquals(0, loggingCommand.getEndCount()); + } assertEquals(isSafe ? 1 : 0, loggingCommand.getExecuteCount()); SafeMode.disable(); @@ -57,9 +57,20 @@ public void testCommand(Function constructor, boolean isSafe, assertTrue(command.isScheduled()); else assertFalse(command.isScheduled()); - if (!staysEnabled) + if (!staysEnabled) { assertEquals(1, loggingCommand.getInitializedCount()); + if(isSafe) + assertEquals(1, loggingCommand.getEndCount()); + else + assertEquals(0, loggingCommand.getEndCount()); + } assertEquals(1, loggingCommand.getExecuteCount()); + + if(!staysEnabled && !isSafe) { + SafeMode.enable(); + CommandScheduler.getInstance().run(); + assertEquals(1, loggingCommand.getEndCount()); + } } }