bugfixes and better tests

Author: CoolSpy3

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

@@ -7,15 +7,27 @@
 
 public class SafeCommand extends FunctionalCommand {
 
+    private final Command command;
+
     public SafeCommand(Command command) {
         super(
-            command::initialize,
-            command::execute,
+            () -> { if(SafeMode.isEnabled()) command.initialize(); },
+            () -> { if(SafeMode.isEnabled()) command.execute(); },
             command::end,
             () -> command.isFinished() || !SafeMode.isEnabled(),
             command.getRequirements().toArray(Subsystem[]::new)
         );
-        CommandScheduler.getInstance().registerComposedCommands(command);
+        CommandScheduler.getInstance().registerComposedCommands(this.command = command);
+    }
+
+    @Override
+    public boolean runsWhenDisabled() {
+        return command.runsWhenDisabled();
+    }
+
+    @Override
+    public InterruptionBehavior getInterruptionBehavior() {
+        return command.getInterruptionBehavior();
     }
 
 }

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

@@ -8,15 +8,27 @@
 // These are suitable for cases where the command should keep running such as a default command where isFinished must always return false
 public class SafeExecuteBlockingCommand extends FunctionalCommand {
 
+    private final Command command;
+
     public SafeExecuteBlockingCommand(Command command) {
         super(
-            command::initialize,
-            () -> { if (SafeMode.isEnabled()) command.execute(); },
+            () -> command.initialize(),
+            () -> { if(SafeMode.isEnabled()) command.execute(); },
             command::end,
             command::isFinished,
             command.getRequirements().toArray(Subsystem[]::new)
         );
-        CommandScheduler.getInstance().registerComposedCommands(command);
+        CommandScheduler.getInstance().registerComposedCommands(this.command = command);
+    }
+
+    @Override
+    public boolean runsWhenDisabled() {
+        return command.runsWhenDisabled();
+    }
+
+    @Override
+    public InterruptionBehavior getInterruptionBehavior() {
+        return command.getInterruptionBehavior();
     }
 
 }

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

@@ -7,15 +7,27 @@
 
 public class UnsafeCommand extends FunctionalCommand {
 
+    private final Command command;
+
     public UnsafeCommand(Command command) {
         super(
-            command::initialize,
-            command::execute,
+            () -> { if(!SafeMode.isEnabled()) command.initialize(); },
+            () -> { if(!SafeMode.isEnabled()) command.execute(); },
             command::end,
             () -> command.isFinished() || SafeMode.isEnabled(),
             command.getRequirements().toArray(Subsystem[]::new)
         );
-        CommandScheduler.getInstance().registerComposedCommands(command);
+        CommandScheduler.getInstance().registerComposedCommands(this.command = command);
+    }
+
+    @Override
+    public boolean runsWhenDisabled() {
+        return command.runsWhenDisabled();
+    }
+
+    @Override
+    public InterruptionBehavior getInterruptionBehavior() {
+        return command.getInterruptionBehavior();
     }
 
 }

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

@@ -8,6 +8,8 @@
 // These are suitable for cases where the command should keep running such as a default command where isFinished must always return false
 public class UnsafeExecuteBlockingCommand extends FunctionalCommand {
 
+    private final Command command;
+
     public UnsafeExecuteBlockingCommand(Command command) {
         super(
             command::initialize,
@@ -16,7 +18,17 @@ public UnsafeExecuteBlockingCommand(Command command) {
             command::isFinished,
             command.getRequirements().toArray(Subsystem[]::new)
         );
-        CommandScheduler.getInstance().registerComposedCommands(command);
+        CommandScheduler.getInstance().registerComposedCommands(this.command = command);
+    }
+
+    @Override
+    public boolean runsWhenDisabled() {
+        return command.runsWhenDisabled();
+    }
+
+    @Override
+    public InterruptionBehavior getInterruptionBehavior() {
+        return command.getInterruptionBehavior();
     }
 
 }

src/test/java/org/carlmontrobotics/lib199/safeMode/LoggingCommand.java

@@ -0,0 +1,41 @@
+package org.carlmontrobotics.lib199.safeMode;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class LoggingCommand extends CommandBase {
+
+    private int initializedCount = 0, executeCount = 0;
+
+    @Override
+    public void initialize() {
+        initializedCount++;
+    }
+
+    @Override
+    public void execute() {
+        executeCount++;
+    }
+
+    public void reset() {
+        initializedCount = executeCount = 0;
+    }
+
+    public int getInitializedCount() {
+        return initializedCount;
+    }
+
+    public int getExecuteCount() {
+        return executeCount;
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    public boolean runsWhenDisabled() {
+        return true;
+    }
+
+}

src/test/java/org/carlmontrobotics/lib199/safeMode/SafeCommandTest.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<Command, Command> 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());
+    }
+
+}