Skip to content

Commit 1f4f7a7

Browse files
CoolSpy3CoolSpy3
committed
safe mode and unit tests
1 parent 432cd16 commit 1f4f7a7

12 files changed

Lines changed: 683 additions & 0 deletions

‎src/main/java/org/carlmontrobotics/lib199/safeMode/SafeCommand.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 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;
7+
8+
public class SafeCommand extends FunctionalCommand {
9+
10+
public SafeCommand(Command command) {
11+
super(
12+
command::initialize,
13+
command::execute,
14+
command::end,
15+
() -> command.isFinished() || !SafeMode.isEnabled(),
16+
command.getRequirements().toArray(Subsystem[]::new)
17+
);
18+
CommandScheduler.getInstance().registerComposedCommands(command);
19+
}
20+
21+
}

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

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
package org.carlmontrobotics.lib199.safeMode;
2+
3+
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;
7+
8+
// These are suitable for cases where the command should keep running such as a default command where isFinished must always return false
9+
public class SafeExecuteBlockingCommand extends FunctionalCommand {
10+
11+
public SafeExecuteBlockingCommand(Command command) {
12+
super(
13+
command::initialize,
14+
() -> { if (SafeMode.isEnabled()) command.execute(); },
15+
command::end,
16+
command::isFinished,
17+
command.getRequirements().toArray(Subsystem[]::new)
18+
);
19+
CommandScheduler.getInstance().registerComposedCommands(command);
20+
}
21+
22+
}

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

Lines changed: 70 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,70 @@
1+
package org.carlmontrobotics.lib199.safeMode;
2+
3+
import java.util.Map;
4+
import java.util.Set;
5+
6+
import edu.wpi.first.wpilibj.GenericHID;
7+
8+
public class SafeJoystick {
9+
10+
public final GenericHID unsafeJoystick;
11+
12+
private final Set<Integer> safeDisabledButtons;
13+
private final Set<Integer> safeDisabledAxes;
14+
private final Map<Integer, Double> safeScaledAxes;
15+
private final Map<Integer, Set<Integer>> safeDisabledPOV;
16+
17+
// Use AtomicBoolean so that the boolean is passed by reference
18+
public SafeJoystick(GenericHID unsafeJoystick, Set<Integer> safeDisabledButtons, Set<Integer> safeDisabledAxes, Map<Integer, Double> safeScaledAxes, Map<Integer, Set<Integer>> safeDisabledPOV) {
19+
this.unsafeJoystick = unsafeJoystick;
20+
this.safeDisabledButtons = safeDisabledButtons;
21+
this.safeDisabledAxes = safeDisabledAxes;
22+
this.safeScaledAxes = safeScaledAxes;
23+
this.safeDisabledPOV = safeDisabledPOV;
24+
}
25+
26+
// All other methods always fall through to these five
27+
28+
public boolean getRawButton(int button) {
29+
if (SafeMode.isEnabled() && safeDisabledButtons.contains(button)) {
30+
return false;
31+
} else {
32+
return unsafeJoystick.getRawButton(button);
33+
}
34+
}
35+
36+
public boolean getRawButtonPressed(int button) {
37+
if (SafeMode.isEnabled() && safeDisabledButtons.contains(button)) {
38+
return false;
39+
} else {
40+
return unsafeJoystick.getRawButtonPressed(button);
41+
}
42+
}
43+
44+
public boolean getRawButtonReleased(int button) {
45+
if (SafeMode.isEnabled() && safeDisabledButtons.contains(button)) {
46+
return false;
47+
} else {
48+
return unsafeJoystick.getRawButtonReleased(button);
49+
}
50+
}
51+
52+
public double getRawAxis(int axis) {
53+
if (SafeMode.isEnabled() && safeDisabledAxes.contains(axis)) {
54+
return 0;
55+
} else if (SafeMode.isEnabled() && safeScaledAxes.containsKey(axis)) {
56+
return unsafeJoystick.getRawAxis(axis) * safeScaledAxes.get(axis);
57+
} else {
58+
return unsafeJoystick.getRawAxis(axis);
59+
}
60+
}
61+
62+
public double getPOV(int pov) {
63+
if (SafeMode.isEnabled() && safeDisabledPOV.containsKey(pov) && safeDisabledPOV.get(pov).contains(unsafeJoystick.getPOV(pov))) {
64+
return -1;
65+
} else {
66+
return unsafeJoystick.getPOV(pov);
67+
}
68+
}
69+
70+
}

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

Lines changed: 174 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,174 @@
1+
package org.carlmontrobotics.lib199.safeMode;
2+
3+
import java.util.Collections;
4+
import java.util.HashSet;
5+
import java.util.Map;
6+
import java.util.Set;
7+
import java.util.concurrent.ConcurrentHashMap;
8+
import java.util.function.BooleanSupplier;
9+
import java.util.function.DoubleSupplier;
10+
import java.util.function.IntSupplier;
11+
import java.util.function.LongSupplier;
12+
import java.util.function.Supplier;
13+
14+
import org.carlmontrobotics.lib199.Lib199Subsystem;
15+
import org.carlmontrobotics.lib199.Mocks;
16+
17+
import edu.wpi.first.networktables.BooleanEntry;
18+
import edu.wpi.first.networktables.BooleanSubscriber;
19+
import edu.wpi.first.networktables.BooleanTopic;
20+
import edu.wpi.first.wpilibj.GenericHID;
21+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
22+
23+
public class SafeMode {
24+
25+
//#region Basic Safe Mode
26+
27+
private static BooleanEntry safeModeStatus;
28+
29+
// NetworkTables doesn't like it when we use the same subscriber for isEnabled() and updateCallbacks()
30+
// My guess is that readQueueValues() interferes with the get() call
31+
private static BooleanSubscriber enabledListener;
32+
33+
static {
34+
SmartDashboard.putBoolean("Safe Mode", false);
35+
BooleanTopic safeModeTopic = new BooleanTopic(SmartDashboard.getEntry("Safe Mode").getTopic());
36+
safeModeStatus = safeModeTopic.getEntry(false);
37+
enabledListener = safeModeTopic.subscribe(false);
38+
39+
// Call updateCallbacks synchronously with the CommandScheduler
40+
Lib199Subsystem.registerPeriodic(SafeMode::updateCallbacks);
41+
}
42+
43+
public static void enable() {
44+
safeModeStatus.set(true);
45+
}
46+
47+
public static void disable() {
48+
safeModeStatus.set(false);
49+
}
50+
51+
public static boolean isEnabled() {
52+
return safeModeStatus.get();
53+
}
54+
55+
//#endregion
56+
57+
//#region Callbacks
58+
59+
private static final Set<Runnable> onSafeModeEnabled = createEmptyThreadSafeSet();
60+
private static final Set<Runnable> onSafeModeDisabled = createEmptyThreadSafeSet();
61+
62+
// Guaranteed to be thread-safe
63+
// Not guaranteed to have non-extraneous call
64+
public static void onEnabled(Runnable runnable) {
65+
onSafeModeEnabled.add(runnable);
66+
}
67+
68+
// Guaranteed to be thread-safe
69+
// Not guaranteed to have non-extraneous call
70+
public static void onDisabled(Runnable runnable) {
71+
onSafeModeDisabled.add(runnable);
72+
}
73+
74+
public static void updateCallbacks() {
75+
boolean stateChanged = enabledListener.readQueueValues().length != 0;
76+
if(stateChanged) {
77+
if (isEnabled()) {
78+
onSafeModeEnabled.forEach(Runnable::run);
79+
} else {
80+
onSafeModeDisabled.forEach(Runnable::run);
81+
}
82+
}
83+
}
84+
85+
//#endregion
86+
87+
//#region Safe Constants
88+
89+
public static <T> Supplier<T> constant(T normalValue, T safeValue) {
90+
return () -> isEnabled() ? safeValue : normalValue;
91+
}
92+
93+
public static BooleanSupplier constant(boolean normalValue, boolean safeValue) {
94+
return () -> isEnabled() ? safeValue : normalValue;
95+
}
96+
97+
public static DoubleSupplier constant(double normalValue, double safeValue) {
98+
return () -> isEnabled() ? safeValue : normalValue;
99+
}
100+
101+
public static IntSupplier constant(int normalValue, int safeValue) {
102+
return () -> isEnabled() ? safeValue : normalValue;
103+
}
104+
105+
public static LongSupplier constant(long normalValue, long safeValue) {
106+
return () -> isEnabled() ? safeValue : normalValue;
107+
}
108+
109+
//#endregion
110+
111+
//#region Safe Joystick
112+
113+
// lib199 uses asynchronous code in a few places, so these will all be thread-safe
114+
private static final Map<Integer, Set<Integer>> safeDisabledButtons = new ConcurrentHashMap<>();
115+
private static final Map<Integer, Set<Integer>> safeDisabledAxes = new ConcurrentHashMap<>();
116+
private static final Map<Integer, Map<Integer, Double>> safeScaledAxes = new ConcurrentHashMap<>();
117+
private static final Map<Integer, Map<Integer, Set<Integer>>> safeDisabledPOVs = new ConcurrentHashMap<>();
118+
119+
@SuppressWarnings("unchecked")
120+
public static <T extends GenericHID> T makeSafe(T joystick) {
121+
safeDisabledAxes.putIfAbsent(joystick.getPort(), createEmptyThreadSafeSet());
122+
safeDisabledButtons.putIfAbsent(joystick.getPort(), createEmptyThreadSafeSet());
123+
safeScaledAxes.putIfAbsent(joystick.getPort(), new ConcurrentHashMap<>());
124+
safeDisabledPOVs.putIfAbsent(joystick.getPort(), new ConcurrentHashMap<>());
125+
126+
int port = joystick.getPort();
127+
return Mocks.createMock(
128+
(Class<T>) joystick.getClass(),
129+
new SafeJoystick(
130+
joystick,
131+
safeDisabledButtons.get(port),
132+
safeDisabledAxes.get(port),
133+
safeScaledAxes.get(port),
134+
safeDisabledPOVs.get(port)
135+
)
136+
);
137+
}
138+
139+
public static void disableButton(int joystickPort, int button) {
140+
safeDisabledButtons.putIfAbsent(joystickPort, createEmptyThreadSafeSet());
141+
safeDisabledButtons.get(joystickPort).add(button);
142+
}
143+
144+
public static void disableAxis(int joystickPort, int axis) {
145+
safeDisabledAxes.putIfAbsent(joystickPort, createEmptyThreadSafeSet());
146+
safeDisabledAxes.get(joystickPort).add(axis);
147+
}
148+
149+
public static void scaleAxis(int joystickPort, int axis, double factor) {
150+
safeScaledAxes.putIfAbsent(joystickPort, new ConcurrentHashMap<>());
151+
safeScaledAxes.get(joystickPort).put(axis, factor);
152+
}
153+
154+
public static void disablePOV(int joystickPort, int angle) {
155+
disablePOV(joystickPort, 0, angle);
156+
}
157+
158+
public static void disablePOV(int joystickPort, int pov, int angle) {
159+
safeDisabledPOVs.putIfAbsent(joystickPort, new ConcurrentHashMap<>());
160+
safeDisabledPOVs.get(joystickPort).putIfAbsent(angle, createEmptyThreadSafeSet());
161+
safeDisabledPOVs.get(joystickPort).get(angle).add(angle);
162+
}
163+
164+
//#endregion
165+
166+
// NOTE: Unlike with maps, there are a few different ways to make sets thread-safe
167+
// I chose this method of creating synchronized sets based on
168+
// https://stackoverflow.com/questions/6720396/different-types-of-thread-safe-sets-in-java, and https://docs.oracle.com/javase/tutorial/collections/implementations/set.html
169+
// Please make your own determination for other areas of the code rather than just copying this
170+
static <T> Set<T> createEmptyThreadSafeSet() {
171+
return Collections.synchronizedSet(new HashSet<>());
172+
}
173+
174+
}

‎src/main/java/org/carlmontrobotics/lib199/safeMode/UnsafeCommand.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 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;
7+
8+
public class UnsafeCommand extends FunctionalCommand {
9+
10+
public UnsafeCommand(Command command) {
11+
super(
12+
command::initialize,
13+
command::execute,
14+
command::end,
15+
() -> command.isFinished() || SafeMode.isEnabled(),
16+
command.getRequirements().toArray(Subsystem[]::new)
17+
);
18+
CommandScheduler.getInstance().registerComposedCommands(command);
19+
}
20+
21+
}

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

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
package org.carlmontrobotics.lib199.safeMode;
2+
3+
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;
7+
8+
// These are suitable for cases where the command should keep running such as a default command where isFinished must always return false
9+
public class UnsafeExecuteBlockingCommand extends FunctionalCommand {
10+
11+
public UnsafeExecuteBlockingCommand(Command command) {
12+
super(
13+
command::initialize,
14+
() -> { if (!SafeMode.isEnabled()) command.execute(); },
15+
command::end,
16+
command::isFinished,
17+
command.getRequirements().toArray(Subsystem[]::new)
18+
);
19+
CommandScheduler.getInstance().registerComposedCommands(command);
20+
}
21+
22+
}

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

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
package org.carlmontrobotics.lib199.safeMode;
2+
3+
import static org.junit.Assert.*;
4+
5+
import org.junit.Test;
6+
7+
import edu.wpi.first.wpilibj2.command.CommandScheduler;
8+
import edu.wpi.first.wpilibj2.command.RunCommand;
9+
10+
public class SafeCommandTest {
11+
12+
@Test
13+
public void testSafeCommand() {
14+
SafeCommand command = new SafeCommand(new RunCommand(() -> {})) {
15+
@Override
16+
public boolean runsWhenDisabled() {
17+
return true;
18+
}
19+
};
20+
21+
SafeMode.enable();
22+
command.schedule();
23+
CommandScheduler.getInstance().run();
24+
assertTrue(command.isScheduled());
25+
26+
SafeMode.disable();
27+
CommandScheduler.getInstance().run();
28+
assertFalse(command.isScheduled());
29+
}
30+
31+
}

0 commit comments

Comments
 (0)