11package org .carlmontrobotics .lib199 ;
22
3- import java .util .ArrayList ;
3+ import java .util .concurrent . CopyOnWriteArrayList ;
44import java .util .function .Consumer ;
55
6+ import edu .wpi .first .wpilibj .RobotBase ;
67import edu .wpi .first .wpilibj2 .command .Subsystem ;
78
89public 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}
0 commit comments