Add Notifier and RobotRunner

dev/roboRIODev/Robot.cs

@@ -4,5 +4,13 @@ namespace TestRobot;
 
 public class Robot : RobotBase
 {
+    public override void EndCompetition()
+    {
+        throw new System.NotImplementedException();
+    }
 
+    public override void StartCompetition()
+    {
+        throw new System.NotImplementedException();
+    }
 }

src/hal/Natives/HalNotifier.cs

@@ -40,7 +40,8 @@ public static partial class HalNotifier
     [AutomateStatusCheck(StatusCheckMethod = HalBase.StatusCheckCall)]
     [LibraryImport("wpiHal", EntryPoint = "HAL_SetNotifierThreadPriority")]
     [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
-    public static partial int SetNotifierThreadPriority(int realTime, int priority, out HalStatus status);
+    [return: MarshalAs(UnmanagedType.I4)]
+    public static partial bool SetNotifierThreadPriority([MarshalAs(UnmanagedType.I4)] bool realTime, int priority, out HalStatus status);
 
     [AutomateStatusCheck(StatusCheckMethod = HalBase.StatusCheckCall)]
     [LibraryImport("wpiHal", EntryPoint = "HAL_WaitForNotifierAlarm")]

src/ntcore/MultiSubscriber.cs

@@ -3,7 +3,7 @@
 
 namespace NetworkTables;
 
-public sealed class MultiSubscriber(NetworkTableInstance inst, string[] prefixes, in PubSubOptions options) : IDisposable
+public sealed class MultiSubscriber(NetworkTableInstance inst, string[] prefixes, PubSubOptions options = default) : IDisposable
 {
     public void Dispose()
     {

src/wpilibsharp/Notifier.cs

@@ -0,0 +1,150 @@
+using UnitsNet;
+using WPIHal.Handles;
+using WPIHal.Natives;
+using WPIUtil;
+
+namespace WPILib;
+
+public class Notifier : IDisposable
+{
+    private Thread m_thread;
+
+    private readonly object m_processLock = new();
+
+    private int m_notifier;
+
+    private Action m_callback;
+
+    private Duration m_expirationTime;
+
+    private Duration m_period;
+
+    private bool m_periodic;
+
+    public void Dispose()
+    {
+        GC.SuppressFinalize(this);
+        HalNotifierHandle handle = new(Interlocked.Exchange(ref m_notifier, 0));
+        if (handle.Handle == 0)
+        {
+            return;
+        }
+
+        HalNotifier.StopNotifier(handle);
+        if (m_thread.IsAlive == true)
+        {
+            m_thread.Interrupt();
+            m_thread.Join();
+        }
+        HalNotifier.CleanNotifier(handle);
+    }
+
+    private void UpdateAlarm(long triggerTime)
+    {
+        HalNotifierHandle handle = new(Interlocked.CompareExchange(ref m_notifier, 0, 0));
+        if (handle.Handle == 0)
+        {
+            return;
+        }
+
+        HalNotifier.UpdateNotifierAlarm(handle, (uint)triggerTime);
+    }
+
+    private void UpdateAlarm()
+    {
+        UpdateAlarm((long)m_expirationTime.Microseconds);
+    }
+
+    public Notifier(Action callback, string name = "Notifier")
+    {
+        m_callback = WpiGuard.RequireNotNull(callback, nameof(callback));
+
+        var notifier = HalNotifier.InitializeNotifier();
+
+        m_notifier = notifier.Handle;
+        HalNotifier.SetNotifierName(notifier, name);
+
+        m_thread = new(() =>
+        {
+            while (true)
+            {
+                HalNotifierHandle handle = new(Interlocked.CompareExchange(ref m_notifier, 0, 0));
+                if (handle.Handle == 0)
+                {
+                    break;
+                }
+                ulong curTime = HalNotifier.WaitForNotifierAlarm(handle);
+                if (curTime == 0)
+                {
+                    break;
+                }
+
+                Action? threadHandler;
+                lock (m_processLock)
+                {
+                    threadHandler = m_callback;
+                    if (m_periodic)
+                    {
+                        m_expirationTime += m_period;
+                        UpdateAlarm();
+                    }
+                    else
+                    {
+                        UpdateAlarm(-1);
+                    }
+                }
+
+                threadHandler?.Invoke();
+            }
+        })
+        {
+            Name = name,
+            IsBackground = true,
+        };
+        m_thread.Start();
+    }
+
+    public void SetCallback(Action callback)
+    {
+        lock (m_processLock)
+        {
+            m_callback = WpiGuard.RequireNotNull(callback, nameof(callback));
+        }
+    }
+
+    public void StartSingle(Duration delay)
+    {
+        lock (m_processLock)
+        {
+            m_periodic = false;
+            m_period = delay;
+            m_expirationTime = Timer.FPGATimestamp + delay;
+            UpdateAlarm();
+        }
+    }
+
+    public void StartPeriodic(Duration period)
+    {
+        lock (m_processLock)
+        {
+            m_periodic = true;
+            m_period = period;
+            m_expirationTime = Timer.FPGATimestamp + period;
+            UpdateAlarm();
+        }
+    }
+
+    public void Stop()
+    {
+        lock (m_processLock)
+        {
+            m_periodic = false;
+            HalNotifier.CancelNotifierAlarm(new(Interlocked.CompareExchange(ref m_notifier, 0, 0)));
+        }
+    }
+
+    public static bool SetHalThreadPriority(bool realTime, int priority)
+    {
+        return HalNotifier.SetNotifierThreadPriority(realTime, priority);
+    }
+}

src/wpilibsharp/RobotBase.cs

@@ -1,8 +1,83 @@
+using System.Runtime.ConstrainedExecution;
+using CommunityToolkit.Diagnostics;
+using NetworkTables;
+using WPIHal.Natives;
+
 namespace WPILib;
 
-public abstract class RobotBase
+public abstract class RobotBase : IDisposable
 {
-    public static bool IsReal => true;
+    private readonly MultiSubscriber m_suball;
+
+    protected RobotBase()
+    {
+        NetworkTableInstance inst = NetworkTableInstance.Default;
+        MainThreadId = Environment.CurrentManagedThreadId;
+        // subscribe = "" to force persistent values to propagate to local
+        m_suball = new MultiSubscriber(inst, [""]);
+        if (!IsSimulation)
+        {
+            inst.StartServer("/home/lvuser/networktables.json");
+        }
+        else
+        {
+            inst.StartServer();
+        }
+
+        // Wait for the NT server to actually start
+        int count = 0;
+        while (inst.GetNetworkMode().HasFlag(NetworkMode.Starting))
+        {
+            Thread.Sleep(10);
+            count++;
+            if (count > 100)
+            {
+                Console.Error.WriteLine("Timed out while waiting for NT server to start");
+                break;
+            }
+        }
+    }
+
+    public static long MainThreadId { get; private set; } = -1;
+
+    public void Dispose()
+    {
+        GC.SuppressFinalize(this);
+        m_suball.Dispose();
+    }
+
+    public static RuntimeType RuntimeType => (RuntimeType)HalBase.GetRuntimeType();
+
+    public static bool IsSimulation => RuntimeType == RuntimeType.Simulation;
+
+    public static bool IsReal
+    {
+        get
+        {
+            var rt = RuntimeType;
+            return rt == RuntimeType.RoboRIO || rt == RuntimeType.RoboRIO2;
+        }
+    }
+
+#pragma warning disable CA1822 // Mark members as static
+    public bool IsDisabled => DriverStation.IsDisabled;
+
+    public bool IsEnabled => DriverStation.IsEnabled;
+
+    public bool IsAutonomous => DriverStation.IsAutonomous;
+
+    public bool IsAutonomousEnabled => DriverStation.IsAutonomousEnabled;
+
+    public bool IsTest => DriverStation.IsTest;
+
+    public bool IsTestEnabled => DriverStation.IsTestEnabled;
+
+    public bool IsTeleop => DriverStation.IsTeleopEnabled;
+
+    public bool IsTeleopEnabled => DriverStation.IsTeleopEnabled;
+#pragma warning restore CA1822 // Mark members as static
+
+    public abstract void StartCompetition();
 
-    public static RuntimeType RuntimeType => RuntimeType.RoboRIO;
+    public abstract void EndCompetition();
 }