Skip to content

Commit 8bc9c58

Browse files
ThadHouseThadHouse
committed
Add Notifier and RobotRunner
1 parent 18eeb02 commit 8bc9c58

File tree

6 files changed

+374
-7
lines changed

6 files changed

+374
-7
lines changed

‎dev/roboRIODev/Robot.cs

+8
Original file line numberDiff line numberDiff line change
@@ -4,5 +4,13 @@ namespace TestRobot;
44

55
public class Robot : RobotBase
66
{
7+
public override void EndCompetition()
8+
{
9+
throw new System.NotImplementedException();
10+
}
711

12+
public override void StartCompetition()
13+
{
14+
throw new System.NotImplementedException();
15+
}
816
}

‎src/hal/Natives/HalNotifier.cs

+2-1
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,8 @@ public static partial class HalNotifier
4040
[AutomateStatusCheck(StatusCheckMethod = HalBase.StatusCheckCall)]
4141
[LibraryImport("wpiHal", EntryPoint = "HAL_SetNotifierThreadPriority")]
4242
[UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
43-
public static partial int SetNotifierThreadPriority(int realTime, int priority, out HalStatus status);
43+
[return: MarshalAs(UnmanagedType.I4)]
44+
public static partial bool SetNotifierThreadPriority([MarshalAs(UnmanagedType.I4)] bool realTime, int priority, out HalStatus status);
4445

4546
[AutomateStatusCheck(StatusCheckMethod = HalBase.StatusCheckCall)]
4647
[LibraryImport("wpiHal", EntryPoint = "HAL_WaitForNotifierAlarm")]

‎src/ntcore/MultiSubscriber.cs

+1-1
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33

44
namespace NetworkTables;
55

6-
public sealed class MultiSubscriber(NetworkTableInstance inst, string[] prefixes, in PubSubOptions options) : IDisposable
6+
public sealed class MultiSubscriber(NetworkTableInstance inst, string[] prefixes, PubSubOptions options = default) : IDisposable
77
{
88
public void Dispose()
99
{

‎src/wpilibsharp/Notifier.cs

+150
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,150 @@
1+
using UnitsNet;
2+
using WPIHal.Handles;
3+
using WPIHal.Natives;
4+
using WPIUtil;
5+
6+
namespace WPILib;
7+
8+
public class Notifier : IDisposable
9+
{
10+
private Thread m_thread;
11+
12+
private readonly object m_processLock = new();
13+
14+
private int m_notifier;
15+
16+
private Action m_callback;
17+
18+
private Duration m_expirationTime;
19+
20+
private Duration m_period;
21+
22+
private bool m_periodic;
23+
24+
public void Dispose()
25+
{
26+
GC.SuppressFinalize(this);
27+
HalNotifierHandle handle = new(Interlocked.Exchange(ref m_notifier, 0));
28+
if (handle.Handle == 0)
29+
{
30+
return;
31+
}
32+
33+
HalNotifier.StopNotifier(handle);
34+
if (m_thread.IsAlive == true)
35+
{
36+
m_thread.Interrupt();
37+
m_thread.Join();
38+
}
39+
HalNotifier.CleanNotifier(handle);
40+
}
41+
42+
private void UpdateAlarm(long triggerTime)
43+
{
44+
HalNotifierHandle handle = new(Interlocked.CompareExchange(ref m_notifier, 0, 0));
45+
if (handle.Handle == 0)
46+
{
47+
return;
48+
}
49+
50+
HalNotifier.UpdateNotifierAlarm(handle, (uint)triggerTime);
51+
}
52+
53+
private void UpdateAlarm()
54+
{
55+
UpdateAlarm((long)m_expirationTime.Microseconds);
56+
}
57+
58+
public Notifier(Action callback, string name = "Notifier")
59+
{
60+
m_callback = WpiGuard.RequireNotNull(callback, nameof(callback));
61+
62+
var notifier = HalNotifier.InitializeNotifier();
63+
64+
m_notifier = notifier.Handle;
65+
HalNotifier.SetNotifierName(notifier, name);
66+
67+
m_thread = new(() =>
68+
{
69+
while (true)
70+
{
71+
HalNotifierHandle handle = new(Interlocked.CompareExchange(ref m_notifier, 0, 0));
72+
if (handle.Handle == 0)
73+
{
74+
break;
75+
}
76+
ulong curTime = HalNotifier.WaitForNotifierAlarm(handle);
77+
if (curTime == 0)
78+
{
79+
break;
80+
}
81+
82+
Action? threadHandler;
83+
lock (m_processLock)
84+
{
85+
threadHandler = m_callback;
86+
if (m_periodic)
87+
{
88+
m_expirationTime += m_period;
89+
UpdateAlarm();
90+
}
91+
else
92+
{
93+
UpdateAlarm(-1);
94+
}
95+
}
96+
97+
threadHandler?.Invoke();
98+
}
99+
})
100+
{
101+
Name = name,
102+
IsBackground = true,
103+
};
104+
m_thread.Start();
105+
}
106+
107+
public void SetCallback(Action callback)
108+
{
109+
lock (m_processLock)
110+
{
111+
m_callback = WpiGuard.RequireNotNull(callback, nameof(callback));
112+
}
113+
}
114+
115+
public void StartSingle(Duration delay)
116+
{
117+
lock (m_processLock)
118+
{
119+
m_periodic = false;
120+
m_period = delay;
121+
m_expirationTime = Timer.FPGATimestamp + delay;
122+
UpdateAlarm();
123+
}
124+
}
125+
126+
public void StartPeriodic(Duration period)
127+
{
128+
lock (m_processLock)
129+
{
130+
m_periodic = true;
131+
m_period = period;
132+
m_expirationTime = Timer.FPGATimestamp + period;
133+
UpdateAlarm();
134+
}
135+
}
136+
137+
public void Stop()
138+
{
139+
lock (m_processLock)
140+
{
141+
m_periodic = false;
142+
HalNotifier.CancelNotifierAlarm(new(Interlocked.CompareExchange(ref m_notifier, 0, 0)));
143+
}
144+
}
145+
146+
public static bool SetHalThreadPriority(bool realTime, int priority)
147+
{
148+
return HalNotifier.SetNotifierThreadPriority(realTime, priority);
149+
}
150+
}

‎src/wpilibsharp/RobotBase.cs

+78-3
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,83 @@
1+
using System.Runtime.ConstrainedExecution;
2+
using CommunityToolkit.Diagnostics;
3+
using NetworkTables;
4+
using WPIHal.Natives;
5+
16
namespace WPILib;
27

3-
public abstract class RobotBase
8+
public abstract class RobotBase : IDisposable
49
{
5-
public static bool IsReal => true;
10+
private readonly MultiSubscriber m_suball;
11+
12+
protected RobotBase()
13+
{
14+
NetworkTableInstance inst = NetworkTableInstance.Default;
15+
MainThreadId = Environment.CurrentManagedThreadId;
16+
// subscribe = "" to force persistent values to propagate to local
17+
m_suball = new MultiSubscriber(inst, [""]);
18+
if (!IsSimulation)
19+
{
20+
inst.StartServer("/home/lvuser/networktables.json");
21+
}
22+
else
23+
{
24+
inst.StartServer();
25+
}
26+
27+
// Wait for the NT server to actually start
28+
int count = 0;
29+
while (inst.GetNetworkMode().HasFlag(NetworkMode.Starting))
30+
{
31+
Thread.Sleep(10);
32+
count++;
33+
if (count > 100)
34+
{
35+
Console.Error.WriteLine("Timed out while waiting for NT server to start");
36+
break;
37+
}
38+
}
39+
}
40+
41+
public static long MainThreadId { get; private set; } = -1;
42+
43+
public void Dispose()
44+
{
45+
GC.SuppressFinalize(this);
46+
m_suball.Dispose();
47+
}
48+
49+
public static RuntimeType RuntimeType => (RuntimeType)HalBase.GetRuntimeType();
50+
51+
public static bool IsSimulation => RuntimeType == RuntimeType.Simulation;
52+
53+
public static bool IsReal
54+
{
55+
get
56+
{
57+
var rt = RuntimeType;
58+
return rt == RuntimeType.RoboRIO || rt == RuntimeType.RoboRIO2;
59+
}
60+
}
61+
62+
#pragma warning disable CA1822 // Mark members as static
63+
public bool IsDisabled => DriverStation.IsDisabled;
64+
65+
public bool IsEnabled => DriverStation.IsEnabled;
66+
67+
public bool IsAutonomous => DriverStation.IsAutonomous;
68+
69+
public bool IsAutonomousEnabled => DriverStation.IsAutonomousEnabled;
70+
71+
public bool IsTest => DriverStation.IsTest;
72+
73+
public bool IsTestEnabled => DriverStation.IsTestEnabled;
74+
75+
public bool IsTeleop => DriverStation.IsTeleopEnabled;
76+
77+
public bool IsTeleopEnabled => DriverStation.IsTeleopEnabled;
78+
#pragma warning restore CA1822 // Mark members as static
79+
80+
public abstract void StartCompetition();
681

7-
public static RuntimeType RuntimeType => RuntimeType.RoboRIO;
82+
public abstract void EndCompetition();
883
}

0 commit comments

Comments
 (0)