Skip to content

Commit ab4f4e2

Browse files
CoolSpy3CoolSpy3
authored
Merge pull request #36 from DeepBlueRobotics/async-periodic
Make lib199 periodic methods run asynchronously
2 parents ed78b8f + 457f12f commit ab4f4e2

9 files changed

Lines changed: 103 additions & 41 deletions

File tree

‎src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java‎

Lines changed: 44 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,21 +1,39 @@
11
package org.carlmontrobotics.lib199;
22

3-
import java.util.ArrayList;
3+
import java.util.concurrent.CopyOnWriteArrayList;
44
import java.util.function.Consumer;
55

6+
import edu.wpi.first.wpilibj.RobotBase;
67
import edu.wpi.first.wpilibj2.command.Subsystem;
78

89
public class Lib199Subsystem implements Subsystem {
910

1011
private static final Lib199Subsystem INSTANCE = new Lib199Subsystem();
11-
private static final ArrayList<Runnable> periodicMethods = new ArrayList<>();
12-
private static final ArrayList<Runnable> periodicSimulationMethods = new ArrayList<>();
12+
private static final CopyOnWriteArrayList<Runnable> periodicMethods = new CopyOnWriteArrayList<>();
13+
private static final CopyOnWriteArrayList<Runnable> periodicSimulationMethods = new CopyOnWriteArrayList<>();
14+
private static final CopyOnWriteArrayList<Runnable> asyncPeriodicMethods = new CopyOnWriteArrayList<>();
15+
private static final CopyOnWriteArrayList<Runnable> asyncPeriodicSimulationMethods = new CopyOnWriteArrayList<>();
1316
private static final Consumer<Runnable> RUN_RUNNABLE = Runnable::run;
1417

18+
private static final Thread asyncPeriodicThread;
19+
20+
public static final long asyncSleepTime = 20;
21+
1522
static {
1623
ensureRegistered();
24+
25+
asyncPeriodicThread = new Thread(() -> {
26+
while(true) {
27+
INSTANCE.asyncPeriodic();
28+
try {
29+
Thread.sleep(asyncSleepTime);
30+
} catch(InterruptedException e) {}
31+
}
32+
});
33+
asyncPeriodicThread.setDaemon(true);
34+
asyncPeriodicThread.start();
1735
}
18-
36+
1937
private static boolean registered = false;
2038

2139
private static void ensureRegistered() {
@@ -30,10 +48,27 @@ public static void registerPeriodic(Runnable method) {
3048
periodicMethods.add(method);
3149
}
3250

51+
@Deprecated
52+
/**
53+
* @deprecated Use registerSimulationPeriodic
54+
* @param method
55+
*/
3356
public static void simulationPeriodic(Runnable method) {
57+
registerSimulationPeriodic(method);
58+
}
59+
60+
public static void registerSimulationPeriodic(Runnable method) {
3461
periodicSimulationMethods.add(method);
3562
}
3663

64+
public static void registerAsyncPeriodic(Runnable method) {
65+
asyncPeriodicMethods.add(method);
66+
}
67+
68+
public static void registerAsyncSimulationPeriodic(Runnable method) {
69+
if(RobotBase.isSimulation()) asyncPeriodicSimulationMethods.add(method);
70+
}
71+
3772
@Override
3873
public void periodic() {
3974
periodicMethods.forEach(RUN_RUNNABLE);
@@ -44,6 +79,11 @@ public void simulationPeriodic() {
4479
periodicSimulationMethods.forEach(RUN_RUNNABLE);
4580
}
4681

82+
public void asyncPeriodic() {
83+
asyncPeriodicMethods.forEach(RUN_RUNNABLE);
84+
asyncPeriodicSimulationMethods.forEach(RUN_RUNNABLE);
85+
}
86+
4787
private Lib199Subsystem() {}
4888

4989
}

‎src/main/java/org/carlmontrobotics/lib199/Mocks.java‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -6,9 +6,8 @@
66
import java.lang.reflect.Modifier;
77
import java.util.ArrayList;
88
import java.util.Arrays;
9-
import java.util.Collections;
109
import java.util.HashMap;
11-
import java.util.List;
10+
import java.util.concurrent.CopyOnWriteArrayList;
1211
import java.util.function.Consumer;
1312
import java.util.function.Predicate;
1413
import java.util.stream.Collectors;
@@ -21,7 +20,7 @@
2120

2221
public final class Mocks {
2322

24-
private static final List<WeakReference<Object>> MOCKS = Collections.synchronizedList(new ArrayList<>());
23+
private static final CopyOnWriteArrayList<WeakReference<Object>> MOCKS = new CopyOnWriteArrayList<>();
2524
private static final Predicate<WeakReference<?>> IS_REFERENCE_CLEARED = reference -> reference.get() == null;
2625
private static final Consumer<WeakReference<Object>> CLEAR_INVOCATIONS_ON_REFERENCED_MOCK = reference -> Mockito.clearInvocations(reference.get());
2726
private static final Predicate<WeakReference<Object>> CLEAR_INVOCATIONS_ON_REFERENCED_MOCK_IF_REFERENCE_NOT_CLEARED = reference -> {
@@ -37,7 +36,7 @@ public final class Mocks {
3736
// 2) Garbage collected references are removed
3837
// 3) Mock is garbage collected
3938
// 4) Mock invocations are cleared -> throws NullPointerException
40-
Lib199Subsystem.registerPeriodic(() -> MOCKS.removeIf(CLEAR_INVOCATIONS_ON_REFERENCED_MOCK_IF_REFERENCE_NOT_CLEARED));
39+
Lib199Subsystem.registerAsyncPeriodic(() -> MOCKS.removeIf(CLEAR_INVOCATIONS_ON_REFERENCED_MOCK_IF_REFERENCE_NOT_CLEARED));
4140
}
4241

4342
/**

‎src/main/java/org/carlmontrobotics/lib199/MotorErrors.java‎

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

3-
import java.util.ArrayList;
43
import java.util.Arrays;
5-
import java.util.HashMap;
4+
import java.util.concurrent.ConcurrentHashMap;
5+
import java.util.concurrent.CopyOnWriteArrayList;
66

77
import com.ctre.phoenix.ErrorCode;
88
import com.revrobotics.CANSparkMax;
@@ -13,14 +13,14 @@
1313

1414
public final class MotorErrors {
1515

16-
private static final HashMap<Integer, CANSparkMax> temperatureSparks = new HashMap<>();
17-
private static final HashMap<Integer, Integer> sparkTemperatureLimits = new HashMap<>();
18-
private static final ArrayList<Integer> overheatedSparks = new ArrayList<>();
19-
private static final HashMap<CANSparkMax, Short> flags = new HashMap<>();
20-
private static final HashMap<CANSparkMax, Short> stickyFlags = new HashMap<>();
16+
private static final ConcurrentHashMap<Integer, CANSparkMax> temperatureSparks = new ConcurrentHashMap<>();
17+
private static final ConcurrentHashMap<Integer, Integer> sparkTemperatureLimits = new ConcurrentHashMap<>();
18+
private static final CopyOnWriteArrayList<Integer> overheatedSparks = new CopyOnWriteArrayList<>();
19+
private static final ConcurrentHashMap<CANSparkMax, Short> flags = new ConcurrentHashMap<>();
20+
private static final ConcurrentHashMap<CANSparkMax, Short> stickyFlags = new ConcurrentHashMap<>();
2121

2222
static {
23-
Lib199Subsystem.registerPeriodic(MotorErrors::doReportSparkMaxTemp);
23+
Lib199Subsystem.registerAsyncPeriodic(MotorErrors::doReportSparkMaxTemp);
2424
}
2525

2626
public static void reportError(ErrorCode error) {

‎src/main/java/org/carlmontrobotics/lib199/sim/MockPhoenixController.java‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
package org.carlmontrobotics.lib199.sim;
22

3-
import java.util.ArrayList;
4-
import java.util.HashMap;
3+
import java.util.concurrent.ConcurrentHashMap;
4+
import java.util.concurrent.CopyOnWriteArrayList;
55

66
import com.ctre.phoenix.motorcontrol.ControlMode;
77
import com.ctre.phoenix.motorcontrol.IMotorController;
@@ -15,7 +15,7 @@ abstract class MockPhoenixController implements AutoCloseable {
1515
// CAN ports should be separate from PWM ports
1616
protected PWMMotorController motorPWM;
1717
// Since we need to keep a record of all the motor's followers
18-
protected static HashMap<Integer, ArrayList<PWMMotorController>> followMap = new HashMap<>();
18+
protected static ConcurrentHashMap<Integer, CopyOnWriteArrayList<PWMMotorController>> followMap = new ConcurrentHashMap<>();
1919

2020
public MockPhoenixController(int portPWM) {
2121
this.portPWM = portPWM;
@@ -36,7 +36,7 @@ public double get() {
3636

3737
public void follow(IMotorController leader) {
3838
if (!followMap.containsKey(leader.getDeviceID())) {
39-
ArrayList<PWMMotorController> arr = new ArrayList<PWMMotorController>();
39+
CopyOnWriteArrayList<PWMMotorController> arr = new CopyOnWriteArrayList<PWMMotorController>();
4040
arr.add(motorPWM);
4141
followMap.put(leader.getDeviceID(), arr);
4242
} else {

‎src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java‎

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

3-
import java.util.ArrayList;
4-
import java.util.HashMap;
3+
import java.util.concurrent.ConcurrentHashMap;
4+
import java.util.concurrent.CopyOnWriteArrayList;
5+
6+
import org.carlmontrobotics.lib199.DummySparkMaxAnswer;
7+
import org.carlmontrobotics.lib199.Mocks;
8+
import org.carlmontrobotics.lib199.REVLibErrorAnswer;
59

6-
import com.revrobotics.REVLibError;
7-
import com.revrobotics.RelativeEncoder;
8-
import com.revrobotics.SparkMaxPIDController;
910
import com.revrobotics.CANSparkMax;
1011
import com.revrobotics.CANSparkMax.ExternalFollower;
1112
import com.revrobotics.CANSparkMax.IdleMode;
1213
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
13-
14-
import org.carlmontrobotics.lib199.DummySparkMaxAnswer;
15-
import org.carlmontrobotics.lib199.Mocks;
16-
import org.carlmontrobotics.lib199.REVLibErrorAnswer;
14+
import com.revrobotics.REVLibError;
15+
import com.revrobotics.RelativeEncoder;
16+
import com.revrobotics.SparkMaxPIDController;
1717

1818
import edu.wpi.first.hal.SimDevice;
19-
import edu.wpi.first.hal.SimDouble;
2019
import edu.wpi.first.hal.SimDevice.Direction;
20+
import edu.wpi.first.hal.SimDouble;
2121

2222
public class MockSparkMax {
2323
// Assign the CAN port to a PWM port so it works with the simulator. Not a fan
@@ -30,7 +30,7 @@ public class MockSparkMax {
3030
private SparkMaxPIDController pidController;
3131
private boolean isInverted;
3232
// Since we need to keep a record of all the motor's followers
33-
private static HashMap<Integer, ArrayList<SimDouble>> followMap = new HashMap<>();
33+
private static ConcurrentHashMap<Integer, CopyOnWriteArrayList<SimDouble>> followMap = new ConcurrentHashMap<>();
3434

3535
public MockSparkMax(int port, MotorType type) {
3636
this.port = port;
@@ -67,7 +67,7 @@ public REVLibError follow(ExternalFollower leader, int deviceID) {
6767

6868
public REVLibError follow(ExternalFollower leader, int deviceID, boolean invert) {
6969
if (!followMap.containsKey(deviceID)) {
70-
followMap.put(deviceID, new ArrayList<SimDouble>());
70+
followMap.put(deviceID, new CopyOnWriteArrayList<SimDouble>());
7171
}
7272
followMap.get(deviceID).add(speed);
7373
return REVLibError.kOk;

‎src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@ public MockedCANCoder(CANCoder canCoder) {
2929
position = device.createDouble("count", Direction.kInput, 0);
3030
gearing = device.createDouble("gearing", Direction.kOutput, 1);
3131
sim = canCoder.getSimCollection();
32-
Lib199Subsystem.registerPeriodic(this::update);
32+
Lib199Subsystem.registerAsyncSimulationPeriodic(this::update);
3333
sims.put(port, this);
3434
}
3535

‎src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkEncoder.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,7 @@ public MockedSparkEncoder(int id) {
3030
count = device.createDouble("count", Direction.kInput, 0);
3131
gearing = device.createDouble("gearing", Direction.kOutput, 1);
3232
sims.put(id, this);
33-
Lib199Subsystem.registerPeriodic(this);
33+
Lib199Subsystem.registerAsyncPeriodic(this);
3434
}
3535

3636
public double getPosition() {

‎src/test/java/org/carlmontrobotics/lib199/Lib199SubsystemTest.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@ public void testPeriodic() {
2525
public void testSimulationPeriodic() {
2626
assumeTrue(RobotBase.isSimulation());
2727
AtomicInteger counter = new AtomicInteger(0);
28-
Lib199Subsystem.registerPeriodic(() -> counter.addAndGet(1));
28+
Lib199Subsystem.registerSimulationPeriodic(() -> counter.addAndGet(1));
2929
assertEquals("Simulation periodic method called before CommandScheduler.run", 0, counter.get());
3030
CommandScheduler.getInstance().run();
3131
assertEquals("Simulation periodic method called more than once or not at all", 1, counter.get());

‎src/test/java/org/carlmontrobotics/lib199/MotorErrorsTest.java‎

Lines changed: 29 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22

33
import static org.junit.Assert.assertEquals;
44
import static org.junit.Assert.assertNotEquals;
5+
import static org.junit.Assume.assumeNoException;
56

67
import com.ctre.phoenix.ErrorCode;
78
import com.revrobotics.REVLibError;
@@ -12,7 +13,6 @@
1213
import org.junit.Test;
1314

1415
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
15-
import edu.wpi.first.wpilibj2.command.CommandScheduler;
1616

1717
public class MotorErrorsTest extends ErrStreamTest {
1818

@@ -77,6 +77,16 @@ public int getDeviceId() {
7777

7878
}
7979

80+
private static final Object asyncPeriodicNotifier = new Object();
81+
82+
static {
83+
Lib199Subsystem.registerAsyncPeriodic(() -> {
84+
synchronized(asyncPeriodicNotifier) {
85+
asyncPeriodicNotifier.notifyAll();
86+
}
87+
});
88+
}
89+
8090
@Test
8191
public void testOkErrors() {
8292
errStream.reset();
@@ -165,30 +175,43 @@ private void doTestReportSparkMaxTemp(int id) {
165175
MotorErrors.reportSparkMaxTemp((CANSparkMax)spark, 40);
166176
spark.setSmartCurrentLimit(50);
167177
spark.setTemperature(20);
168-
CommandScheduler.getInstance().run();
178+
runAsyncPeriodic();
169179
String smartDashboardKey = "Port " + id + " Spark Max Temp";
170180
assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01);
171181
assertEquals(50, spark.getSmartCurrentLimit());
172182
spark.setTemperature(20);
173-
CommandScheduler.getInstance().run();
183+
runAsyncPeriodic();
174184
assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01);
175185
assertEquals(50, spark.getSmartCurrentLimit());
176186
assertEquals(0, errStream.size());
177187
spark.setTemperature(40);
178-
CommandScheduler.getInstance().run();
188+
runAsyncPeriodic();
179189
assertEquals(40, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01);
180190
assertEquals(1, spark.getSmartCurrentLimit());
181191
assertNotEquals(0, errStream.size());
182192
errStream.reset();
183193
spark.setTemperature(50);
184-
CommandScheduler.getInstance().run();
194+
runAsyncPeriodic();
185195
assertEquals(50, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01);
186196
assertEquals(1, spark.getSmartCurrentLimit());
187197
spark.setTemperature(20);
188-
CommandScheduler.getInstance().run();
198+
runAsyncPeriodic();
189199
assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01);
190200
assertEquals(1, spark.getSmartCurrentLimit());
191201
assertEquals(0, errStream.size());
192202
}
193203

204+
// Ensures an update to the asynchronous periodic thread is run
205+
private void runAsyncPeriodic() {
206+
try {
207+
synchronized(asyncPeriodicNotifier) {
208+
// Run twice because we don't know in what order we're called, so make sure all periodic methods are run twice
209+
asyncPeriodicNotifier.wait();
210+
asyncPeriodicNotifier.wait();
211+
}
212+
} catch(InterruptedException e) {
213+
assumeNoException(e);
214+
}
215+
}
216+
194217
}

0 commit comments

Comments
 (0)