Skip to content

Commit 8097741

Browse files
CoolSpy3CoolSpy3
authored
Merge pull request #60 from DeepBlueRobotics/safe-mode-2
Better Safe-Mode Commands
2 parents 3569b17 + 8beb140 commit 8097741

8 files changed

Lines changed: 128 additions & 92 deletions

File tree

‎src/main/java/org/carlmontrobotics/lib199/safeMode/EndBlockingCommand.java‎

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
package org.carlmontrobotics.lib199.safeMode;
2+
3+
import edu.wpi.first.wpilibj2.command.Command;
4+
import edu.wpi.first.wpilibj2.command.WrapperCommand;
5+
6+
/**
7+
* A command who's {@link #end(boolean)} method can only be called if its {@link #initialize()} method has been called first.
8+
*/
9+
public class EndBlockingCommand extends WrapperCommand {
10+
11+
private boolean initialized = false;
12+
13+
/**
14+
* Creates a new EndBlockingCommand
15+
* @param command The command to run
16+
*/
17+
public EndBlockingCommand(Command command) {
18+
super(command);
19+
}
20+
21+
@Override
22+
public void initialize() {
23+
synchronized(this) {
24+
initialized = true;
25+
}
26+
super.initialize();
27+
}
28+
29+
@Override
30+
public void end(boolean interrupted) {
31+
synchronized(this) {
32+
if(initialized) {
33+
super.end(interrupted);
34+
initialized = false;
35+
}
36+
}
37+
}
38+
39+
}

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

Lines changed: 13 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -1,18 +1,12 @@
11
package org.carlmontrobotics.lib199.safeMode;
22

33
import edu.wpi.first.wpilibj2.command.Command;
4-
import edu.wpi.first.wpilibj2.command.CommandScheduler;
5-
import edu.wpi.first.wpilibj2.command.FunctionalCommand;
6-
import edu.wpi.first.wpilibj2.command.Subsystem;
4+
import edu.wpi.first.wpilibj2.command.WrapperCommand;
75

86
/**
9-
* A command that only runs when safe-mode is enabled and returns {@code isFinished() = true} otherwise.
10-
*
11-
* 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})
7+
* 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.
128
*/
13-
public class SafeCommand extends FunctionalCommand {
14-
15-
private final Command command;
9+
public class SafeCommand extends WrapperCommand {
1610

1711
static {
1812
SafeMode.ensureInitialized();
@@ -23,24 +17,22 @@ public class SafeCommand extends FunctionalCommand {
2317
* @param command The command to run
2418
*/
2519
public SafeCommand(Command command) {
26-
super(
27-
() -> { if(SafeMode.isEnabled()) command.initialize(); },
28-
() -> { if(SafeMode.isEnabled()) command.execute(); },
29-
command::end,
30-
() -> command.isFinished() || !SafeMode.isEnabled(),
31-
command.getRequirements().toArray(Subsystem[]::new)
32-
);
33-
CommandScheduler.getInstance().registerComposedCommands(this.command = command);
20+
super(new EndBlockingCommand(command));
3421
}
3522

3623
@Override
37-
public boolean runsWhenDisabled() {
38-
return command.runsWhenDisabled();
24+
public void initialize() {
25+
if(SafeMode.isEnabled()) super.initialize();
3926
}
4027

4128
@Override
42-
public InterruptionBehavior getInterruptionBehavior() {
43-
return command.getInterruptionBehavior();
29+
public void execute() {
30+
if(SafeMode.isEnabled()) super.execute();
4431
}
4532

33+
@Override
34+
public boolean isFinished() {
35+
if(!SafeMode.isEnabled()) end(true);
36+
return super.isFinished() || !SafeMode.isEnabled();
37+
}
4638
}

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

Lines changed: 5 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -1,43 +1,27 @@
11
package org.carlmontrobotics.lib199.safeMode;
22

33
import edu.wpi.first.wpilibj2.command.Command;
4-
import edu.wpi.first.wpilibj2.command.CommandScheduler;
5-
import edu.wpi.first.wpilibj2.command.FunctionalCommand;
6-
import edu.wpi.first.wpilibj2.command.Subsystem;
4+
import edu.wpi.first.wpilibj2.command.WrapperCommand;
75

86
/**
97
* 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.
108
*
119
* 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}.
1210
* It is intended for cases where the command should keep running such as a default command where {@link #isFinished()} must always return {@code false}
1311
*/
14-
public class SafeExecuteBlockingCommand extends FunctionalCommand {
15-
16-
private final Command command;
12+
public class SafeExecuteBlockingCommand extends WrapperCommand {
1713

1814
static {
1915
SafeMode.ensureInitialized();
2016
}
2117

2218
public SafeExecuteBlockingCommand(Command command) {
23-
super(
24-
() -> command.initialize(),
25-
() -> { if(SafeMode.isEnabled()) command.execute(); },
26-
command::end,
27-
command::isFinished,
28-
command.getRequirements().toArray(Subsystem[]::new)
29-
);
30-
CommandScheduler.getInstance().registerComposedCommands(this.command = command);
31-
}
32-
33-
@Override
34-
public boolean runsWhenDisabled() {
35-
return command.runsWhenDisabled();
19+
super(command);
3620
}
3721

3822
@Override
39-
public InterruptionBehavior getInterruptionBehavior() {
40-
return command.getInterruptionBehavior();
23+
public void execute() {
24+
if(SafeMode.isEnabled()) super.execute();
4125
}
4226

4327
}

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

Lines changed: 14 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -1,42 +1,35 @@
11
package org.carlmontrobotics.lib199.safeMode;
22

33
import edu.wpi.first.wpilibj2.command.Command;
4-
import edu.wpi.first.wpilibj2.command.CommandScheduler;
5-
import edu.wpi.first.wpilibj2.command.FunctionalCommand;
6-
import edu.wpi.first.wpilibj2.command.Subsystem;
4+
import edu.wpi.first.wpilibj2.command.WrapperCommand;
75

86
/**
9-
* A command that only runs when safe-mode is disabled and returns {@code isFinished() = true} otherwise.
10-
*
11-
* 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})
7+
* 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.
128
*/
13-
public class UnsafeCommand extends FunctionalCommand {
14-
15-
private final Command command;
9+
public class UnsafeCommand extends WrapperCommand {
1610

1711
static {
1812
SafeMode.ensureInitialized();
1913
}
2014

2115
public UnsafeCommand(Command command) {
22-
super(
23-
() -> { if(!SafeMode.isEnabled()) command.initialize(); },
24-
() -> { if(!SafeMode.isEnabled()) command.execute(); },
25-
command::end,
26-
() -> command.isFinished() || SafeMode.isEnabled(),
27-
command.getRequirements().toArray(Subsystem[]::new)
28-
);
29-
CommandScheduler.getInstance().registerComposedCommands(this.command = command);
16+
super(new EndBlockingCommand(command));
17+
}
18+
19+
@Override
20+
public void initialize() {
21+
if(!SafeMode.isEnabled()) super.initialize();
3022
}
3123

3224
@Override
33-
public boolean runsWhenDisabled() {
34-
return command.runsWhenDisabled();
25+
public void execute() {
26+
if(!SafeMode.isEnabled()) super.execute();
3527
}
3628

3729
@Override
38-
public InterruptionBehavior getInterruptionBehavior() {
39-
return command.getInterruptionBehavior();
30+
public boolean isFinished() {
31+
if(SafeMode.isEnabled()) end(true);
32+
return SafeMode.isEnabled() || super.isFinished();
4033
}
4134

4235
}

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

Lines changed: 5 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -1,43 +1,27 @@
11
package org.carlmontrobotics.lib199.safeMode;
22

33
import edu.wpi.first.wpilibj2.command.Command;
4-
import edu.wpi.first.wpilibj2.command.CommandScheduler;
5-
import edu.wpi.first.wpilibj2.command.FunctionalCommand;
6-
import edu.wpi.first.wpilibj2.command.Subsystem;
4+
import edu.wpi.first.wpilibj2.command.WrapperCommand;
75

86
/**
97
* 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.
108
*
119
* 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}.
1210
* It is intended for cases where the command should keep running such as a default command where {@link #isFinished()} must always return {@code false}
1311
*/
14-
public class UnsafeExecuteBlockingCommand extends FunctionalCommand {
15-
16-
private final Command command;
12+
public class UnsafeExecuteBlockingCommand extends WrapperCommand {
1713

1814
static {
1915
SafeMode.ensureInitialized();
2016
}
2117

2218
public UnsafeExecuteBlockingCommand(Command command) {
23-
super(
24-
command::initialize,
25-
() -> { if (!SafeMode.isEnabled()) command.execute(); },
26-
command::end,
27-
command::isFinished,
28-
command.getRequirements().toArray(Subsystem[]::new)
29-
);
30-
CommandScheduler.getInstance().registerComposedCommands(this.command = command);
31-
}
32-
33-
@Override
34-
public boolean runsWhenDisabled() {
35-
return command.runsWhenDisabled();
19+
super(command);
3620
}
3721

3822
@Override
39-
public InterruptionBehavior getInterruptionBehavior() {
40-
return command.getInterruptionBehavior();
23+
public void execute() {
24+
if (!SafeMode.isEnabled()) super.execute();
4125
}
4226

4327
}

‎src/test/java/org/carlmontrobotics/lib199/safeMode/EndBlockingCommandTest.java‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
package org.carlmontrobotics.lib199.safeMode;
2+
3+
import static org.junit.Assert.assertEquals;
4+
5+
import org.junit.Test;
6+
7+
public class EndBlockingCommandTest {
8+
9+
@Test
10+
public void testEndBlockingCommand() {
11+
LoggingCommand loggingCommand = new LoggingCommand();
12+
EndBlockingCommand command = new EndBlockingCommand(loggingCommand);
13+
loggingCommand.reset();
14+
command.initialize();
15+
command.end(false);
16+
assertEquals(1, loggingCommand.getEndCount());
17+
command.end(false);
18+
assertEquals(1, loggingCommand.getEndCount());
19+
}
20+
21+
}

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

Lines changed: 15 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -3,13 +3,13 @@
33
import edu.wpi.first.wpilibj2.command.CommandBase;
44

55
/**
6-
* A command that counts how many times it has been initialized and executed for the purposes of testing safe mode functionality.
6+
* A command that counts how many times it has been initialized, executed, and ended for the purposes of testing safe mode functionality.
77
*
88
* @see SafeModeCommandsTest
99
*/
1010
public class LoggingCommand extends CommandBase {
1111

12-
private int initializedCount = 0, executeCount = 0;
12+
private int initializedCount = 0, executeCount = 0, endCount = 0;
1313

1414
@Override
1515
public void initialize() {
@@ -21,11 +21,16 @@ public void execute() {
2121
executeCount++;
2222
}
2323

24+
@Override
25+
public void end(boolean interrupted) {
26+
endCount++;
27+
}
28+
2429
/**
2530
* Resets the initialized and execute counts to zero.
2631
*/
2732
public void reset() {
28-
initializedCount = executeCount = 0;
33+
initializedCount = executeCount = endCount = 0;
2934
}
3035

3136
/**
@@ -42,6 +47,13 @@ public int getExecuteCount() {
4247
return executeCount;
4348
}
4449

50+
/**
51+
* @return The number of times this command has been ended.
52+
*/
53+
public int getEndCount() {
54+
return endCount;
55+
}
56+
4557
@Override
4658
public boolean isFinished() {
4759
return false;

‎src/test/java/org/carlmontrobotics/lib199/safeMode/SafeModeCommandsTest.java‎

Lines changed: 16 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,6 @@
11
package org.carlmontrobotics.lib199.safeMode;
22

3-
import static org.junit.Assert.assertEquals;
4-
import static org.junit.Assert.assertFalse;
5-
import static org.junit.Assert.assertTrue;
3+
import static org.junit.Assert.*;
64

75
import java.util.function.Function;
86

@@ -46,8 +44,10 @@ public void testCommand(Function<Command, Command> constructor, boolean isSafe,
4644
assertTrue(command.isScheduled());
4745
else
4846
assertFalse(command.isScheduled());
49-
if (!staysEnabled)
47+
if (!staysEnabled) {
5048
assertEquals(isSafe ? 1 : 0, loggingCommand.getInitializedCount());
49+
assertEquals(0, loggingCommand.getEndCount());
50+
}
5151
assertEquals(isSafe ? 1 : 0, loggingCommand.getExecuteCount());
5252

5353
SafeMode.disable();
@@ -57,9 +57,20 @@ public void testCommand(Function<Command, Command> constructor, boolean isSafe,
5757
assertTrue(command.isScheduled());
5858
else
5959
assertFalse(command.isScheduled());
60-
if (!staysEnabled)
60+
if (!staysEnabled) {
6161
assertEquals(1, loggingCommand.getInitializedCount());
62+
if(isSafe)
63+
assertEquals(1, loggingCommand.getEndCount());
64+
else
65+
assertEquals(0, loggingCommand.getEndCount());
66+
}
6267
assertEquals(1, loggingCommand.getExecuteCount());
68+
69+
if(!staysEnabled && !isSafe) {
70+
SafeMode.enable();
71+
CommandScheduler.getInstance().run();
72+
assertEquals(1, loggingCommand.getEndCount());
73+
}
6374
}
6475

6576
}

0 commit comments

Comments
 (0)