Better Safe-Mode Commands

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;
+            }
+        }
+    }
+
+}

src/main/java/org/carlmontrobotics/lib199/safeMode/SafeCommand.java

@@ -1,18 +1,12 @@
 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 {
 
     static {
         SafeMode.ensureInitialized();
@@ -23,24 +17,22 @@ public class SafeCommand extends FunctionalCommand {
      * @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();
+    }
 }

src/main/java/org/carlmontrobotics/lib199/safeMode/SafeExecuteBlockingCommand.java

@@ -1,43 +1,27 @@
 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.
  *
  * 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 {
 
     static {
         SafeMode.ensureInitialized();
     }
 
     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();
     }
 
 }

src/main/java/org/carlmontrobotics/lib199/safeMode/UnsafeCommand.java

@@ -1,42 +1,35 @@
 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 {
 
     static {
         SafeMode.ensureInitialized();
     }
 
     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();
     }
 
 }

src/main/java/org/carlmontrobotics/lib199/safeMode/UnsafeExecuteBlockingCommand.java

@@ -1,43 +1,27 @@
 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.
  *
  * 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 {
 
     static {
         SafeMode.ensureInitialized();
     }
 
     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();
     }
 
 }

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());
+    }
+
+}

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;

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<Command, Command> 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<Command, Command> 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());
+        }
     }
 
 }