From 7e22e0194ea4e79563dd7ec453cbb7479525c92f Mon Sep 17 00:00:00 2001 From: Thad House Date: Tue, 19 Mar 2024 16:07:12 -0400 Subject: [PATCH] Add most of the DS class --- src/hal/JoystickAxis.cs | 19 +- src/hal/JoystickButtons.cs | 26 +- src/hal/JoystickPOVs.cs | 11 +- src/hal/MatchInfo.cs | 13 +- src/hal/Natives/HalBase.cs | 20 +- src/hal/Natives/HalDriverStation.cs | 11 +- src/ntcore/Generated/BooleanTopic.cs | 22 +- src/ntcore/Generated/IntegerTopic.cs | 22 +- src/ntcore/Strings/StringTopic.cs | 22 +- src/wpilibsharp/DataLogManager.cs | 3 +- src/wpilibsharp/DriverStation.cs | 854 +++++++++++++++++++++- src/wpilibsharp/RobotController.cs | 20 +- src/wpilibsharp/RobotRunner.cs | 5 +- src/wpilibsharp/Timer.cs | 89 +++ src/wpiutil/Concurrent/Synchronization.cs | 29 +- 15 files changed, 1102 insertions(+), 64 deletions(-) create mode 100644 src/wpilibsharp/Timer.cs diff --git a/src/hal/JoystickAxis.cs b/src/hal/JoystickAxis.cs index c5d2ee24..879fb8d5 100644 --- a/src/hal/JoystickAxis.cs +++ b/src/hal/JoystickAxis.cs @@ -1,3 +1,4 @@ +using System.Diagnostics.CodeAnalysis; using System.Runtime.InteropServices; namespace WPIHal; @@ -5,13 +6,15 @@ namespace WPIHal; [StructLayout(LayoutKind.Sequential)] public readonly struct JoystickAxes { - [System.Runtime.CompilerServices.InlineArray(12)] + public const int NumJoystickAxes = 12; + + [System.Runtime.CompilerServices.InlineArray(NumJoystickAxes)] public struct AxesBuffer { private float _element0; } - [System.Runtime.CompilerServices.InlineArray(12)] + [System.Runtime.CompilerServices.InlineArray(NumJoystickAxes)] public struct RawBuffer { private byte _element0; @@ -22,6 +25,12 @@ public struct RawBuffer public readonly int Count => m_count; + [UnscopedRef] + public ReadOnlySpan AxesSpan => m_axes[..Count]; + + [UnscopedRef] + public ReadOnlySpan RawSpan => m_raw[..Count]; + public readonly double? this[int index] { get @@ -49,4 +58,10 @@ public readonly double? this[int index] } } + + public bool IsEqual(ref readonly JoystickAxes other) + { + // Assumed that if raw is equal, floats are equal + return m_count == other.Count && RawSpan.SequenceEqual(other.RawSpan); + } } diff --git a/src/hal/JoystickButtons.cs b/src/hal/JoystickButtons.cs index 8dff2d6f..bbbcb0dc 100644 --- a/src/hal/JoystickButtons.cs +++ b/src/hal/JoystickButtons.cs @@ -5,20 +5,29 @@ namespace WPIHal; [StructLayout(LayoutKind.Sequential)] public readonly struct JoystickButtons { - private readonly uint buttons; - private readonly byte count; + private readonly uint m_buttons; + private readonly byte m_count; - public int Count => count; + public int Count => m_count; - public uint Buttons => buttons; + public uint Buttons => m_buttons; + + public ReadOnlySpan ToSpan(Span storage) + { + for (int i = 0; i < m_count; i++) + { + storage[i] = (m_buttons & i) != 0; + } + return storage[..m_count]; + } public bool? this[int index] { get { - if (index < count) + if (index < m_count) { - return (buttons & index) != 0; + return (m_buttons & index) != 0; } else { @@ -26,4 +35,9 @@ public bool? this[int index] } } } + + public bool IsEqual(JoystickButtons other) + { + return m_count == other.m_count && m_buttons == other.m_buttons; + } } diff --git a/src/hal/JoystickPOVs.cs b/src/hal/JoystickPOVs.cs index 20d33899..8e0ba34a 100644 --- a/src/hal/JoystickPOVs.cs +++ b/src/hal/JoystickPOVs.cs @@ -1,3 +1,4 @@ +using System.Diagnostics.CodeAnalysis; using System.Runtime.InteropServices; namespace WPIHal; @@ -5,8 +6,9 @@ namespace WPIHal; [StructLayout(LayoutKind.Sequential)] public readonly struct JoystickPOVs { + public const int NumJoystickPOVs = 12; - [System.Runtime.CompilerServices.InlineArray(12)] + [System.Runtime.CompilerServices.InlineArray(NumJoystickPOVs)] public struct PovsBuffer { private short _element0; @@ -17,6 +19,9 @@ public struct PovsBuffer public readonly int Count => m_count; + [UnscopedRef] + public ReadOnlySpan PovsSpan => m_povs[..Count]; + public readonly int? this[int index] { get @@ -32,4 +37,8 @@ public readonly int? this[int index] } } + public bool IsEqual(ref readonly JoystickPOVs other) + { + return m_count == other.Count && PovsSpan.SequenceEqual(other.PovsSpan); + } } diff --git a/src/hal/MatchInfo.cs b/src/hal/MatchInfo.cs index f30e3a9e..a2f0a408 100644 --- a/src/hal/MatchInfo.cs +++ b/src/hal/MatchInfo.cs @@ -1,11 +1,12 @@ using System.Runtime.InteropServices; using System.Runtime.InteropServices.Marshalling; +using System.Text; namespace WPIHal; [NativeMarshalling(typeof(MatchInfoMarshaller))] [StructLayout(LayoutKind.Auto)] -public record struct MatchInfo(string EventName, MatchType MatchType, int MatchNumber, int ReplayNumber, byte[] GameSpecificMessage); +public record struct MatchInfo(string EventName, MatchType MatchType, int MatchNumber, int ReplayNumber, string GameSpecificMessage); [CustomMarshaller(typeof(MatchInfo), MarshalMode.ManagedToUnmanagedOut, typeof(MatchInfoMarshaller))] [CustomMarshaller(typeof(MatchInfo), MarshalMode.ManagedToUnmanagedIn, typeof(MatchInfoMarshaller))] @@ -19,7 +20,7 @@ public static unsafe MatchInfo ConvertToManaged(NativeMatchInfo unmanaged) MatchType = unmanaged.matchType, MatchNumber = unmanaged.matchNumber, ReplayNumber = unmanaged.replayNumber, - GameSpecificMessage = unmanaged.gameSpecificMessage.FromRawBytes(unmanaged.gameSpecificMessageSize) + GameSpecificMessage = unmanaged.gameSpecificMessage.FromByteString(unmanaged.gameSpecificMessageSize) }; } @@ -49,9 +50,15 @@ public readonly unsafe byte[] FromRawBytes(int length) { byte[] ret = new byte[int.Min(length, 64)]; ReadOnlySpan thisSpan = this; - thisSpan[ret.Length..].CopyTo(ret.AsSpan()); + thisSpan[..ret.Length].CopyTo(ret.AsSpan()); return ret; } + + public readonly unsafe string FromByteString(int length) + { + ReadOnlySpan thisSpan = this; + return Encoding.UTF8.GetString(thisSpan[..length]); + } } public Utf8StringBuffer eventName; diff --git a/src/hal/Natives/HalBase.cs b/src/hal/Natives/HalBase.cs index 6aad1d0d..81d7197a 100644 --- a/src/hal/Natives/HalBase.cs +++ b/src/hal/Natives/HalBase.cs @@ -2,6 +2,7 @@ using System.Runtime.InteropServices; using System.Runtime.InteropServices.Marshalling; using System.Text; +using CommunityToolkit.Diagnostics; using WPIHal.Handles; using WPIHal.Marshal; using WPIUtil; @@ -16,7 +17,15 @@ public static partial class HalBase [LibraryImport("wpiHal", EntryPoint = "HAL_Initialize")] [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])] [return: MarshalAs(UnmanagedType.I4)] - public static partial bool Initialize(int timeout, int mode); + internal static partial bool Initialize(int timeout, int mode); + + public static void Initialize() + { + if (!Initialize(500, 0)) + { + ThrowHelper.ThrowInvalidOperationException("HAL failed to initialize"); + } + } [AutomateStatusCheck(StatusCheckMethod = StatusCheckCall)] [LibraryImport("wpiHal", EntryPoint = "HAL_GetFPGATime")] @@ -31,7 +40,8 @@ public static partial class HalBase [AutomateStatusCheck(StatusCheckMethod = StatusCheckCall)] [LibraryImport("wpiHal", EntryPoint = "HAL_GetBrownedOut")] [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])] - public static partial int GetBrownedOut(out HalStatus status); + [return: MarshalAs(UnmanagedType.I4)] + public static partial bool GetBrownedOut(out HalStatus status); [LibraryImport("wpiHal", EntryPoint = "HAL_GetErrorMessage")] [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])] @@ -41,7 +51,8 @@ public static partial class HalBase [AutomateStatusCheck(StatusCheckMethod = StatusCheckCall)] [LibraryImport("wpiHal", EntryPoint = "HAL_GetFPGAButton")] [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])] - public static partial int GetFPGAButton(out HalStatus status); + [return: MarshalAs(UnmanagedType.I4)] + public static partial bool GetFPGAButton(out HalStatus status); [AutomateStatusCheck(StatusCheckMethod = StatusCheckCall)] [LibraryImport("wpiHal", EntryPoint = "HAL_GetFPGARevision")] @@ -68,7 +79,8 @@ public static partial class HalBase [AutomateStatusCheck(StatusCheckMethod = StatusCheckCall)] [LibraryImport("wpiHal", EntryPoint = "HAL_GetSystemActive")] [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])] - public static partial int GetSystemActive(out HalStatus status); + [return: MarshalAs(UnmanagedType.I4)] + public static partial bool GetSystemActive(out HalStatus status); [LibraryImport("wpiHal", EntryPoint = "HAL_GetLastError")] [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])] diff --git a/src/hal/Natives/HalDriverStation.cs b/src/hal/Natives/HalDriverStation.cs index f3b8b538..cf88e1c7 100644 --- a/src/hal/Natives/HalDriverStation.cs +++ b/src/hal/Natives/HalDriverStation.cs @@ -18,13 +18,17 @@ public static partial class HalDriverStation [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])] public static partial HalStatus GetControlWord(out ControlWord controlWord); + [LibraryImport("wpiHal", EntryPoint = "HAL_GetControlWord")] + [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])] + public static unsafe partial HalStatus GetControlWordNative(uint* controlWord); + [LibraryImport("wpiHal", EntryPoint = "HAL_GetJoystickAxes")] [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])] public static partial HalStatus GetJoystickAxes(int joystickNum, out JoystickAxes axes); [LibraryImport("wpiHal", EntryPoint = "HAL_GetJoystickAxisType")] [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])] - public static partial HalStatus GetJoystickAxisType(int joystickNum, int axis); + public static partial int GetJoystickAxisType(int joystickNum, int axis); [LibraryImport("wpiHal", EntryPoint = "HAL_GetJoystickButtons")] [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])] @@ -36,7 +40,8 @@ public static partial class HalDriverStation [LibraryImport("wpiHal", EntryPoint = "HAL_GetJoystickIsXbox")] [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])] - public static partial HalStatus GetJoystickIsXbox(int joystickNum); + [return: MarshalAs(UnmanagedType.I4)] + public static partial bool GetJoystickIsXbox(int joystickNum); [LibraryImport("wpiHal", EntryPoint = "HAL_GetJoystickName")] [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])] @@ -48,7 +53,7 @@ public static partial class HalDriverStation [LibraryImport("wpiHal", EntryPoint = "HAL_GetJoystickType")] [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])] - public static partial HalStatus GetJoystickType(int joystickNum); + public static partial int GetJoystickType(int joystickNum); [LibraryImport("wpiHal", EntryPoint = "HAL_GetMatchInfo")] [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])] diff --git a/src/ntcore/Generated/BooleanTopic.cs b/src/ntcore/Generated/BooleanTopic.cs index a9285e04..2f672ee8 100644 --- a/src/ntcore/Generated/BooleanTopic.cs +++ b/src/ntcore/Generated/BooleanTopic.cs @@ -54,7 +54,7 @@ public BooleanTopic(NetworkTableInstance inst, NtTopic handle) : base(inst, hand /// subscriber public IBooleanSubscriber Subscribe( bool defaultValue, - PubSubOptions options) + PubSubOptions options = default) { return new BooleanEntryImpl( this, @@ -83,7 +83,7 @@ public IBooleanSubscriber Subscribe( public IBooleanSubscriber SubscribeEx( string typeString, bool defaultValue, - PubSubOptions options) + PubSubOptions options = default) { return new BooleanEntryImpl( this, @@ -112,7 +112,7 @@ public IBooleanSubscriber SubscribeEx( public IBooleanSubscriber SubscribeEx( ReadOnlySpan typeString, bool defaultValue, - PubSubOptions options) + PubSubOptions options = default) { return new BooleanEntryImpl( this, @@ -137,7 +137,7 @@ public IBooleanSubscriber SubscribeEx( /// publish options /// publisher public IBooleanPublisher Publish( - PubSubOptions options) + PubSubOptions options = default) { return new BooleanEntryImpl( this, @@ -165,7 +165,7 @@ public IBooleanPublisher Publish( /// publisher public IBooleanPublisher PublishEx( string typeString, string properties, - PubSubOptions options) + PubSubOptions options = default) { return new BooleanEntryImpl( this, @@ -194,7 +194,7 @@ public IBooleanPublisher PublishEx( public IBooleanPublisher PublishEx( ReadOnlySpan typeString, string properties, - PubSubOptions options) + PubSubOptions options = default) { return new BooleanEntryImpl( this, @@ -223,7 +223,7 @@ public IBooleanPublisher PublishEx( public IBooleanPublisher PublishEx( string typeString, ReadOnlySpan properties, - PubSubOptions options) + PubSubOptions options = default) { return new BooleanEntryImpl( this, @@ -252,7 +252,7 @@ public IBooleanPublisher PublishEx( public IBooleanPublisher PublishEx( ReadOnlySpan typeString, ReadOnlySpan properties, - PubSubOptions options) + PubSubOptions options = default) { return new BooleanEntryImpl( this, @@ -284,7 +284,7 @@ public IBooleanPublisher PublishEx( /// entry public IBooleanEntry GetEntry( bool defaultValue, - PubSubOptions options) + PubSubOptions options = default) { return new BooleanEntryImpl( this, @@ -318,7 +318,7 @@ public IBooleanEntry GetEntry( public IBooleanEntry GetEntryEx( string typeString, bool defaultValue, - PubSubOptions options) + PubSubOptions options = default) { return new BooleanEntryImpl( this, @@ -352,7 +352,7 @@ public IBooleanEntry GetEntryEx( public IBooleanEntry GetEntryEx( ReadOnlySpan typeString, bool defaultValue, - PubSubOptions options) + PubSubOptions options = default) { return new BooleanEntryImpl( this, diff --git a/src/ntcore/Generated/IntegerTopic.cs b/src/ntcore/Generated/IntegerTopic.cs index 3ac038cb..a1d28758 100644 --- a/src/ntcore/Generated/IntegerTopic.cs +++ b/src/ntcore/Generated/IntegerTopic.cs @@ -54,7 +54,7 @@ public IntegerTopic(NetworkTableInstance inst, NtTopic handle) : base(inst, hand /// subscriber public IIntegerSubscriber Subscribe( long defaultValue, - PubSubOptions options) + PubSubOptions options = default) { return new IntegerEntryImpl( this, @@ -83,7 +83,7 @@ public IIntegerSubscriber Subscribe( public IIntegerSubscriber SubscribeEx( string typeString, long defaultValue, - PubSubOptions options) + PubSubOptions options = default) { return new IntegerEntryImpl( this, @@ -112,7 +112,7 @@ public IIntegerSubscriber SubscribeEx( public IIntegerSubscriber SubscribeEx( ReadOnlySpan typeString, long defaultValue, - PubSubOptions options) + PubSubOptions options = default) { return new IntegerEntryImpl( this, @@ -137,7 +137,7 @@ public IIntegerSubscriber SubscribeEx( /// publish options /// publisher public IIntegerPublisher Publish( - PubSubOptions options) + PubSubOptions options = default) { return new IntegerEntryImpl( this, @@ -165,7 +165,7 @@ public IIntegerPublisher Publish( /// publisher public IIntegerPublisher PublishEx( string typeString, string properties, - PubSubOptions options) + PubSubOptions options = default) { return new IntegerEntryImpl( this, @@ -194,7 +194,7 @@ public IIntegerPublisher PublishEx( public IIntegerPublisher PublishEx( ReadOnlySpan typeString, string properties, - PubSubOptions options) + PubSubOptions options = default) { return new IntegerEntryImpl( this, @@ -223,7 +223,7 @@ public IIntegerPublisher PublishEx( public IIntegerPublisher PublishEx( string typeString, ReadOnlySpan properties, - PubSubOptions options) + PubSubOptions options = default) { return new IntegerEntryImpl( this, @@ -252,7 +252,7 @@ public IIntegerPublisher PublishEx( public IIntegerPublisher PublishEx( ReadOnlySpan typeString, ReadOnlySpan properties, - PubSubOptions options) + PubSubOptions options = default) { return new IntegerEntryImpl( this, @@ -284,7 +284,7 @@ public IIntegerPublisher PublishEx( /// entry public IIntegerEntry GetEntry( long defaultValue, - PubSubOptions options) + PubSubOptions options = default) { return new IntegerEntryImpl( this, @@ -318,7 +318,7 @@ public IIntegerEntry GetEntry( public IIntegerEntry GetEntryEx( string typeString, long defaultValue, - PubSubOptions options) + PubSubOptions options = default) { return new IntegerEntryImpl( this, @@ -352,7 +352,7 @@ public IIntegerEntry GetEntryEx( public IIntegerEntry GetEntryEx( ReadOnlySpan typeString, long defaultValue, - PubSubOptions options) + PubSubOptions options = default) { return new IntegerEntryImpl( this, diff --git a/src/ntcore/Strings/StringTopic.cs b/src/ntcore/Strings/StringTopic.cs index dd09951f..62c10ffe 100644 --- a/src/ntcore/Strings/StringTopic.cs +++ b/src/ntcore/Strings/StringTopic.cs @@ -54,7 +54,7 @@ public StringTopic(NetworkTableInstance inst, NtTopic handle) : base(inst, handl /// subscriber public IStringSubscriber Subscribe( string defaultValue, - PubSubOptions options) + PubSubOptions options = default) { return new StringEntryImpl( this, @@ -83,7 +83,7 @@ public IStringSubscriber Subscribe( public IStringSubscriber SubscribeEx( string typeString, string defaultValue, - PubSubOptions options) + PubSubOptions options = default) { return new StringEntryImpl( this, @@ -112,7 +112,7 @@ public IStringSubscriber SubscribeEx( public IStringSubscriber SubscribeEx( ReadOnlySpan typeString, string defaultValue, - PubSubOptions options) + PubSubOptions options = default) { return new StringEntryImpl( this, @@ -137,7 +137,7 @@ public IStringSubscriber SubscribeEx( /// publish options /// publisher public IStringPublisher Publish( - PubSubOptions options) + PubSubOptions options = default) { return new StringEntryImpl( this, @@ -165,7 +165,7 @@ public IStringPublisher Publish( /// publisher public IStringPublisher PublishEx( string typeString, string properties, - PubSubOptions options) + PubSubOptions options = default) { return new StringEntryImpl( this, @@ -194,7 +194,7 @@ public IStringPublisher PublishEx( public IStringPublisher PublishEx( ReadOnlySpan typeString, string properties, - PubSubOptions options) + PubSubOptions options = default) { return new StringEntryImpl( this, @@ -223,7 +223,7 @@ public IStringPublisher PublishEx( public IStringPublisher PublishEx( string typeString, ReadOnlySpan properties, - PubSubOptions options) + PubSubOptions options = default) { return new StringEntryImpl( this, @@ -252,7 +252,7 @@ public IStringPublisher PublishEx( public IStringPublisher PublishEx( ReadOnlySpan typeString, ReadOnlySpan properties, - PubSubOptions options) + PubSubOptions options = default) { return new StringEntryImpl( this, @@ -284,7 +284,7 @@ public IStringPublisher PublishEx( /// entry public IStringEntry GetEntry( string defaultValue, - PubSubOptions options) + PubSubOptions options = default) { return new StringEntryImpl( this, @@ -318,7 +318,7 @@ public IStringEntry GetEntry( public IStringEntry GetEntryEx( string typeString, string defaultValue, - PubSubOptions options) + PubSubOptions options = default) { return new StringEntryImpl( this, @@ -354,7 +354,7 @@ public IStringEntry GetEntryEx( #pragma warning restore CA1711 // Identifiers should not have incorrect suffix ReadOnlySpan typeString, string defaultValue, - PubSubOptions options) + PubSubOptions options = default) { return new StringEntryImpl( this, diff --git a/src/wpilibsharp/DataLogManager.cs b/src/wpilibsharp/DataLogManager.cs index 0888101a..7b0fcb0d 100644 --- a/src/wpilibsharp/DataLogManager.cs +++ b/src/wpilibsharp/DataLogManager.cs @@ -2,6 +2,7 @@ using System.Text; using NetworkTables; using NetworkTables.Handles; +using UnitsNet.NumberExtensions.NumberToDuration; using WPIUtil.Concurrent; using WPIUtil.Logging; @@ -292,7 +293,7 @@ private static void LogMain() DriverStation.ProvideRefreshedDataEventHandle(newDataEvent.Handle); while (Interlocked.CompareExchange(ref m_thread, null, null) != null) { - var result = Synchronization.WaitForObject(newDataEvent.Handle.Handle, TimeSpan.FromSeconds(0.25)); + var result = Synchronization.WaitForObject(newDataEvent.Handle.Handle, 0.25.Seconds()); if (result == SynchronizationResult.Cancelled || Interlocked.CompareExchange(ref m_thread, null, null) != null) { break; diff --git a/src/wpilibsharp/DriverStation.cs b/src/wpilibsharp/DriverStation.cs index 2d688154..be5358bc 100644 --- a/src/wpilibsharp/DriverStation.cs +++ b/src/wpilibsharp/DriverStation.cs @@ -1,19 +1,869 @@ +using CommunityToolkit.Diagnostics; +using NetworkTables; +using UnitsNet; +using UnitsNet.NumberExtensions.NumberToDuration; +using WPIHal; +using WPIHal.Natives; +using WPIUtil; +using WPIUtil.Concurrent; using WPIUtil.Handles; +using WPIUtil.Logging; +using WPIUtil.Natives; +using MatchType = WPIHal.MatchType; namespace WPILib; public static class DriverStation { + private static readonly Duration JoystickUnpluggedMessageInterval = 1.0.Seconds(); + private static Duration m_nextMessageTime = 0.Seconds(); + + public enum AllianceColor + { + Red, + Blue + } + + private sealed class MatchDataSender + { + NetworkTable table; + IStringPublisher typeMetadata; + IStringPublisher gameSpecificMessage; + IStringPublisher eventName; + IIntegerPublisher matchNumber; + IIntegerPublisher replayNumber; + IIntegerPublisher matchType; + IBooleanPublisher alliance; + IIntegerPublisher station; + IIntegerPublisher controlWord; + bool oldIsRedAlliance = true; + int oldStationNumber = 1; + string oldEventName = ""; + string oldGameSpecificMessage = ""; + int oldMatchNumber; + int oldReplayNumber; + int oldMatchType; + uint oldControlWord; + + public MatchDataSender() + { + table = NetworkTableInstance.Default.GetTable("FMSInfo"); + typeMetadata = table.GetStringTopic(".type").Publish(); + typeMetadata.Set("FMSInfo"); + gameSpecificMessage = table.GetStringTopic("GameSpecificMessage").Publish(); + gameSpecificMessage.Set(""); + eventName = table.GetStringTopic("EventName").Publish(); + eventName.Set(""); + matchNumber = table.GetIntegerTopic("MatchNumber").Publish(); + matchNumber.Set(0); + replayNumber = table.GetIntegerTopic("ReplayNumber").Publish(); + replayNumber.Set(0); + matchType = table.GetIntegerTopic("MatchType").Publish(); + matchType.Set(0); + alliance = table.GetBooleanTopic("IsRedAlliance").Publish(); + alliance.Set(true); + station = table.GetIntegerTopic("StationNumber").Publish(); + station.Set(1); + controlWord = table.GetIntegerTopic("FMSControlData").Publish(); + controlWord.Set(0); + } + + public void SendMatchData() + { + var allianceID = HalDriverStation.GetAllianceStation(); + + bool isRedAlliance; + int stationNumber; + switch (allianceID) + { + case AllianceStationID.Blue1: + isRedAlliance = false; + stationNumber = 1; + break; + case AllianceStationID.Blue2: + isRedAlliance = false; + stationNumber = 2; + break; + case AllianceStationID.Blue3: + isRedAlliance = false; + stationNumber = 3; + break; + case AllianceStationID.Red1: + isRedAlliance = true; + stationNumber = 1; + break; + case AllianceStationID.Red2: + isRedAlliance = true; + stationNumber = 2; + break; + default: + isRedAlliance = true; + stationNumber = 3; + break; + } + + string currentEventName; + string currentGameSpecificMessage; + int currentMatchNumber; + int currentReplayNumber; + int currentMatchType; + uint currentControlWord; + + lock (m_cacheDataMutex) + { + currentEventName = m_matchInfo.EventName; + currentGameSpecificMessage = m_matchInfo.GameSpecificMessage; + currentMatchNumber = m_matchInfo.MatchNumber; + currentReplayNumber = m_matchInfo.ReplayNumber; + currentMatchType = (int)m_matchInfo.MatchType; + } + + unsafe + { + HalDriverStation.GetControlWordNative(¤tControlWord); + } + + if (oldIsRedAlliance != isRedAlliance) + { + alliance.Set(isRedAlliance); + oldIsRedAlliance = isRedAlliance; + } + if (oldStationNumber != stationNumber) + { + station.Set(stationNumber); + oldStationNumber = stationNumber; + } + if (oldEventName != currentEventName) + { + eventName.Set(currentEventName); + oldEventName = currentEventName; + } + if (oldGameSpecificMessage != currentGameSpecificMessage) + { + gameSpecificMessage.Set(currentGameSpecificMessage); + oldGameSpecificMessage = currentGameSpecificMessage; + } + if (currentMatchNumber != oldMatchNumber) + { + matchNumber.Set(currentMatchNumber); + oldMatchNumber = currentMatchNumber; + } + if (currentReplayNumber != oldReplayNumber) + { + replayNumber.Set(currentReplayNumber); + oldReplayNumber = currentReplayNumber; + } + if (currentMatchType != oldMatchType) + { + matchType.Set(currentMatchType); + oldMatchType = currentMatchType; + } + if (currentControlWord != oldControlWord) + { + controlWord.Set(currentControlWord); + oldControlWord = currentControlWord; + } + + } + } + + private sealed class JoystickLogSender + { + readonly int m_stick; + JoystickButtons m_prevButtons; + JoystickAxes m_prevAxes; + JoystickPOVs m_prevPOVs; + readonly BooleanArrayLogEntry m_logButtons; + readonly FloatArrayLogEntry m_logAxes; + readonly IntegerArrayLogEntry m_logPOVs; + + public JoystickLogSender(DataLog log, int stick, long timestamp) + { + m_stick = stick; + + m_logButtons = new BooleanArrayLogEntry(log, $"DS:joystick{stick}/buttons", timestamp: timestamp); + m_logAxes = new FloatArrayLogEntry(log, $"DS:joystick{stick}/axes", timestamp: timestamp); + m_logPOVs = new IntegerArrayLogEntry(log, $"DS:joystick{stick}/povs", timestamp: timestamp); + + AppendButtons(m_joystickButtons[m_stick], timestamp); + AppendAxes(in m_joystickAxes[m_stick], timestamp); + AppendPOVs(in m_joystickPOVs[m_stick], timestamp); + } + + public void Send(long timestamp) + { + var buttons = m_joystickButtons[m_stick]; + if (!buttons.IsEqual(m_prevButtons)) + { + AppendButtons(buttons, timestamp); + } + + // Ref due to copy size + ref var axes = ref m_joystickAxes[m_stick]; + if (!axes.IsEqual(in m_prevAxes)) + { + AppendAxes(in axes, timestamp); + } + + // Ref due to copy size + ref var povs = ref m_joystickPOVs[m_stick]; + if (!povs.IsEqual(in m_prevPOVs)) + { + AppendPOVs(in povs, timestamp); + } + } + + void AppendButtons(JoystickButtons buttons, long timestamp) + { + var count = buttons.Count; + Span buttonSpan = stackalloc bool[count]; + + m_logButtons.Append(buttons.ToSpan(buttonSpan), timestamp); + m_prevButtons = buttons; + } + + void AppendAxes(ref readonly JoystickAxes axes, long timestamp) + { + ReadOnlySpan axesSpan = axes.AxesSpan; + m_logAxes.Append(axesSpan, timestamp); + m_prevAxes = axes; + } + + void AppendPOVs(ref readonly JoystickPOVs povs, long timestamp) + { + ReadOnlySpan povSpan = povs.PovsSpan; + Span longSpan = stackalloc long[povSpan.Length]; + for (int i = 0; i < povSpan.Length; i++) + { + longSpan[i] = povSpan[i]; + } + m_logPOVs.Append(longSpan, timestamp); + m_prevPOVs = povs; + } + } + + private sealed class DataLogSender + { + public DataLogSender(DataLog log, bool logJoysticks, long timestamp) + { + m_logEnabled = new BooleanLogEntry(log, "DS:enabled", timestamp: timestamp); + m_logAutonomous = new BooleanLogEntry(log, "DS:autonomous", timestamp: timestamp); + m_logTest = new BooleanLogEntry(log, "DS:test", timestamp: timestamp); + m_logEstop = new BooleanLogEntry(log, "DS:estop", timestamp: timestamp); + + m_wasEnabled = m_controlWordCache.Enabled; + m_wasAutonomous = m_controlWordCache.Autonomous; + m_wasTest = m_controlWordCache.Test; + m_wasEstop = m_controlWordCache.EStop; + + m_logEnabled.Append(m_wasEnabled, timestamp); + m_logAutonomous.Append(m_wasAutonomous, timestamp); + m_logTest.Append(m_wasTest, timestamp); + m_logEstop.Append(m_wasEstop, timestamp); + + if (logJoysticks) + { + m_joysticks = new JoystickLogSender[NumJoystickPorts]; + for (int i = 0; i < NumJoystickPorts; i++) + { + m_joysticks[i] = new(log, i, timestamp); + } + } + else + { + m_joysticks = []; + } + } + + public void Send(long timestamp) + { + // append control word value changes + bool enabled = m_controlWordCache.Enabled; + if (enabled != m_wasEnabled) + { + m_logEnabled.Append(enabled, timestamp); + } + m_wasEnabled = enabled; + + bool autonomous = m_controlWordCache.Autonomous; + if (autonomous != m_wasAutonomous) + { + m_logAutonomous.Append(autonomous, timestamp); + } + m_wasAutonomous = autonomous; + + bool test = m_controlWordCache.Test; + if (test != m_wasTest) + { + m_logTest.Append(test, timestamp); + } + m_wasTest = test; + + bool estop = m_controlWordCache.EStop; + if (estop != m_wasEstop) + { + m_logEstop.Append(estop, timestamp); + } + m_wasEstop = estop; + + // append joystick value changes + foreach (JoystickLogSender joystick in m_joysticks) + { + joystick.Send(timestamp); + } + } + + bool m_wasEnabled; + bool m_wasAutonomous; + bool m_wasTest; + bool m_wasEstop; + + readonly BooleanLogEntry m_logEnabled; + readonly BooleanLogEntry m_logAutonomous; + readonly BooleanLogEntry m_logTest; + readonly BooleanLogEntry m_logEstop; + + readonly JoystickLogSender[] m_joysticks; + } + + private const int NumJoystickPorts = 6; + + private static JoystickAxes[] m_joystickAxes = new JoystickAxes[NumJoystickPorts]; + private static JoystickPOVs[] m_joystickPOVs = new JoystickPOVs[NumJoystickPorts]; + private static JoystickButtons[] m_joystickButtons = new JoystickButtons[NumJoystickPorts]; + private static MatchInfo m_matchInfo; + private static ControlWord m_controlWord; + + private static JoystickAxes[] m_joystickAxesCache = new JoystickAxes[NumJoystickPorts]; + private static JoystickPOVs[] m_joystickPOVsCache = new JoystickPOVs[NumJoystickPorts]; + private static JoystickButtons[] m_joystickButtonsCache = new JoystickButtons[NumJoystickPorts]; + private static MatchInfo m_matchInfoCache; + private static ControlWord m_controlWordCache; + + private static EventVector m_refreshEvents = new(); + + private static int[] m_joystickButtonsPressed = new int[NumJoystickPorts]; + private static int[] m_joystickButtonsReleased = new int[NumJoystickPorts]; + + private static readonly MatchDataSender m_matchDataSender; + private static DataLogSender? m_dataLogSender; + private static bool m_silenceJoystickConnectionWarning; + private static readonly object m_cacheDataMutex = new(); + + static DriverStation() + { + HalBase.Initialize(); + + m_matchDataSender = new(); + } + + private static void ReportJoystickUnpluggedError(string message) + { + var currentTime = Timer.FPGATimestamp; + if (currentTime > m_nextMessageTime) + { + ReportError(message, false); + m_nextMessageTime = currentTime + JoystickUnpluggedMessageInterval; + } + } + + private static void ReportJoystickUnpluggedWarning(string message) + { + if (SilenceJoystickConnectionWarning) + { + return; + } + var currentTime = Timer.FPGATimestamp; + if (currentTime > m_nextMessageTime) + { + ReportWarning(message, false); + m_nextMessageTime = currentTime + JoystickUnpluggedMessageInterval; + } + } + + public static bool GetStickButton(int stick, int button) + { + if (stick < 0 || stick >= NumJoystickPorts) + { + ThrowHelper.ThrowArgumentOutOfRangeException(nameof(stick), $"Joystick index {stick} is out of range, should be 0-5"); + } + + if (button <= 0) + { + ReportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C#"); + return false; + } + + lock (m_cacheDataMutex) + { + var result = m_joystickButtons[stick][button]; + if (result is { } r) + { + return r; + } + } + + ReportJoystickUnpluggedWarning($"Joystick Button {button} on port {stick} not available, check if controller is plugged in"); + return false; + } + + public static double GetStickAxis(int stick, int axis) + { + if (stick < 0 || stick >= NumJoystickPorts) + { + ThrowHelper.ThrowArgumentOutOfRangeException(nameof(stick), $"Joystick index {stick} is out of range, should be 0-5"); + } + + if (axis < 0 || axis >= JoystickAxes.NumJoystickAxes) + { + ThrowHelper.ThrowArgumentOutOfRangeException(nameof(axis), $"Joystick axis {axis} is out of range"); + } + + lock (m_cacheDataMutex) + { + var result = m_joystickAxes[stick][axis]; + if (result is { } r) + { + return r; + } + } + + ReportJoystickUnpluggedWarning($"Joystick Axis {axis} on port {stick} not available, check if controller is plugged in"); + return 0.0; + } + + public static int GetStickPOV(int stick, int pov) + { + if (stick < 0 || stick >= NumJoystickPorts) + { + ThrowHelper.ThrowArgumentOutOfRangeException(nameof(stick), $"Joystick index {stick} is out of range, should be 0-5"); + } + + if (pov < 0 || pov >= JoystickPOVs.NumJoystickPOVs) + { + ThrowHelper.ThrowArgumentOutOfRangeException(nameof(pov), $"Joystick pov {pov} is out of range"); + } + + lock (m_cacheDataMutex) + { + var result = m_joystickPOVs[stick][pov]; + if (result is { } r) + { + return r; + } + } + + ReportJoystickUnpluggedWarning($"Joystick POV {pov} on port {stick} not available, check if controller is plugged in"); + return -1; + } + + public static uint GetStickButtons(int stick) + { + if (stick < 0 || stick >= NumJoystickPorts) + { + ThrowHelper.ThrowArgumentOutOfRangeException(nameof(stick), $"Joystick index {stick} is out of range, should be 0-5"); + } + + lock (m_cacheDataMutex) + { + return m_joystickButtons[stick].Buttons; + } + } + + public static int GetStickAxisCount(int stick) + { + if (stick < 0 || stick >= NumJoystickPorts) + { + ThrowHelper.ThrowArgumentOutOfRangeException(nameof(stick), $"Joystick index {stick} is out of range, should be 0-5"); + } + + lock (m_cacheDataMutex) + { + return m_joystickAxes[stick].Count; + } + } + + public static int GetStickPOVCount(int stick) + { + if (stick < 0 || stick >= NumJoystickPorts) + { + ThrowHelper.ThrowArgumentOutOfRangeException(nameof(stick), $"Joystick index {stick} is out of range, should be 0-5"); + } + + lock (m_cacheDataMutex) + { + return m_joystickPOVs[stick].Count; + } + } + + public static int GetStickButtonCount(int stick) + { + if (stick < 0 || stick >= NumJoystickPorts) + { + ThrowHelper.ThrowArgumentOutOfRangeException(nameof(stick), $"Joystick index {stick} is out of range, should be 0-5"); + } + + lock (m_cacheDataMutex) + { + return m_joystickButtons[stick].Count; + } + } + + public static bool GetJoystickIsXbox(int stick) + { + if (stick < 0 || stick >= NumJoystickPorts) + { + ThrowHelper.ThrowArgumentOutOfRangeException(nameof(stick), $"Joystick index {stick} is out of range, should be 0-5"); + } + + return HalDriverStation.GetJoystickIsXbox(stick); + } + + public static int GetJoystickType(int stick) + { + if (stick < 0 || stick >= NumJoystickPorts) + { + ThrowHelper.ThrowArgumentOutOfRangeException(nameof(stick), $"Joystick index {stick} is out of range, should be 0-5"); + } + + return HalDriverStation.GetJoystickType(stick); + } + + public static string GetJoystickName(int stick) + { + if (stick < 0 || stick >= NumJoystickPorts) + { + ThrowHelper.ThrowArgumentOutOfRangeException(nameof(stick), $"Joystick index {stick} is out of range, should be 0-5"); + } + + HalDriverStation.GetJoystickName(out var name, stick); + return name; + } + + public static int GetJoystickAxisType(int stick, int axis) + { + if (stick < 0 || stick >= NumJoystickPorts) + { + ThrowHelper.ThrowArgumentOutOfRangeException(nameof(stick), $"Joystick index {stick} is out of range, should be 0-5"); + } + + return HalDriverStation.GetJoystickAxisType(stick, axis); + } + + public static bool IsJoystickConnected(int stick) + { + return GetStickAxisCount(stick) > 0 || GetStickButtonCount(stick) > 0 || GetStickPOVCount(stick) > 0; + } + + public static bool IsEnabled + { + get + { + lock (m_cacheDataMutex) + { + return m_controlWord.Enabled && m_controlWord.DsAttached; + } + } + } + + public static bool IsDisabled => !IsEnabled; + + public static bool IsEStopped + { + get + { + lock (m_cacheDataMutex) + { + return m_controlWord.EStop; + } + } + } + + public static bool IsAutonomous + { + get + { + lock (m_cacheDataMutex) + { + return m_controlWord.Autonomous; + } + } + } + + public static bool IsAutonomousEnabled + { + get + { + lock (m_cacheDataMutex) + { + return m_controlWord.Autonomous && m_controlWord.Enabled; + } + } + } + + public static bool IsTeleop + { + get + { + lock (m_cacheDataMutex) + { + return !(m_controlWord.Autonomous || m_controlWord.Test); + } + } + } + + public static bool IsTeleopEnabled + { + get + { + lock (m_cacheDataMutex) + { + return !m_controlWord.Autonomous && !m_controlWord.Test && m_controlWord.Enabled; + } + } + } + + public static bool IsTest + { + get + { + lock (m_cacheDataMutex) + { + return m_controlWord.Test; + } + } + } + + public static bool IsTestEnabled + { + get + { + lock (m_cacheDataMutex) + { + return m_controlWord.Test && m_controlWord.Enabled; + } + } + } + + public static bool IsDSAttached + { + get + { + lock (m_cacheDataMutex) + { + return m_controlWord.DsAttached; + } + } + } + + public static bool IsFMSAttached + { + get + { + lock (m_cacheDataMutex) + { + return m_controlWord.FmsAttached; + } + } + } + + public static string GameSpecificMessage + { + get + { + lock (m_cacheDataMutex) + { + return m_matchInfo.GameSpecificMessage; + } + } + } + + public static string EventName + { + get + { + lock (m_cacheDataMutex) + { + return m_matchInfo.EventName; + } + } + } + + public static MatchType MatchType + { + get + { + lock (m_cacheDataMutex) + { + return m_matchInfo.MatchType; + } + } + } + + public static int MatchNumber + { + get + { + lock (m_cacheDataMutex) + { + return m_matchInfo.MatchNumber; + } + } + } + + public static int ReplayNumber + { + get + { + lock (m_cacheDataMutex) + { + return m_matchInfo.ReplayNumber; + } + } + } + + public static AllianceColor? Alliance + { + get + { + var alliance = HalDriverStation.GetAllianceStation(out var _); + return alliance switch + { + AllianceStationID.Red1 => AllianceColor.Red, + AllianceStationID.Red2 => AllianceColor.Red, + AllianceStationID.Red3 => AllianceColor.Red, + AllianceStationID.Blue1 => AllianceColor.Blue, + AllianceStationID.Blue2 => AllianceColor.Blue, + AllianceStationID.Blue3 => AllianceColor.Blue, + _ => null, + }; + } + } + + public static int? Location + { + get + { + var alliance = HalDriverStation.GetAllianceStation(out var _); + return alliance switch + { + AllianceStationID.Red1 => 1, + AllianceStationID.Red2 => 2, + AllianceStationID.Red3 => 3, + AllianceStationID.Blue1 => 1, + AllianceStationID.Blue2 => 2, + AllianceStationID.Blue3 => 3, + _ => null, + }; + } + } + + public static AllianceStationID RawAllianceStation => HalDriverStation.GetAllianceStation(out var _); + + public static bool WaitForDsConnection(Duration timeout) + { + using WpiEvent wpiEvent = new(); + try + { + HalDriverStation.ProvideNewDataEventHandle(wpiEvent.Handle); + SynchronizationResult result; + if (timeout.Seconds <= 0) + { + result = Synchronization.WaitForObject(wpiEvent); + } + else + { + result = Synchronization.WaitForObject(wpiEvent, timeout); + } + return result == SynchronizationResult.Signaled; + } + finally + { + HalDriverStation.RemoveNewDataEventHandle(wpiEvent.Handle); + } + } + + public static double MatchTime => HalDriverStation.GetMatchTime(out var _); + + public static bool SilenceJoystickConnectionWarning + { + get => !IsFMSAttached && m_silenceJoystickConnectionWarning; + set => m_silenceJoystickConnectionWarning = value; + } + + public static ControlWord GetControlWordFromCache() + { + lock (m_cacheDataMutex) + { + return m_controlWord; + } + } + + public static void RefreshData() + { + HalDriverStation.RefrehshDSData(); + + // Get the status of all the joysticks + for (int stick = 0; stick < NumJoystickPorts; stick++) + { + HalDriverStation.GetJoystickAxes(stick, out m_joystickAxesCache[stick]); + HalDriverStation.GetJoystickPOVs(stick, out m_joystickPOVsCache[stick]); + HalDriverStation.GetJoystickButtons(stick, out m_joystickButtonsCache[stick]); + } + + HalDriverStation.GetMatchInfo(out m_matchInfoCache); + HalDriverStation.GetControlWord(out m_controlWordCache); + + DataLogSender? dataLogSender; + + // lock joystick mutex to swap cache data + lock (m_cacheDataMutex) + { + // move cache to actual data + (m_joystickAxesCache, m_joystickAxes) = (m_joystickAxes, m_joystickAxesCache); + (m_joystickButtonsCache, m_joystickButtons) = (m_joystickButtons, m_joystickButtonsCache); + (m_joystickPOVsCache, m_joystickPOVs) = (m_joystickPOVs, m_joystickPOVsCache); + (m_matchInfoCache, m_matchInfo) = (m_matchInfo, m_matchInfoCache); + (m_controlWordCache, m_controlWord) = (m_controlWord, m_controlWordCache); + dataLogSender = m_dataLogSender; + } + + m_refreshEvents.Wakeup(); + + m_matchDataSender.SendMatchData(); + dataLogSender?.Send((long)TimestampNative.Now()); + } + public static void ReportWarning(string warning, bool printTrace) { } + public static void ReportError(string warning, bool printTrace) + { + + } + public static void ProvideRefreshedDataEventHandle(WpiEventHandle handle) { + m_refreshEvents.Add(handle.Handle); + } + + public static void RemoveRefreshedDataEventHandle(WpiEventHandle handle) + { + m_refreshEvents.Remove(handle.Handle); + } + public static void StartDataLog(DataLog log, bool logJoysticks) + { + lock (m_cacheDataMutex) + { + m_dataLogSender ??= new(log, logJoysticks, (long)TimestampNative.Now()); + } } - public static bool IsDSAttached => true; - public static bool IsFMSAttached => true; + public static void StartDataLog(DataLog log) + { + StartDataLog(log, true); + } } diff --git a/src/wpilibsharp/RobotController.cs b/src/wpilibsharp/RobotController.cs index bd648971..6d931a0f 100644 --- a/src/wpilibsharp/RobotController.cs +++ b/src/wpilibsharp/RobotController.cs @@ -1,6 +1,24 @@ +using UnitsNet; +using UnitsNet.NumberExtensions.NumberToElectricCurrent; +using UnitsNet.NumberExtensions.NumberToElectricPotential; +using WPIHal.Natives; + namespace WPILib; public static class RobotController { - public static bool IsSystemTimeValid => true; + public static int FPGAVersion => HalBase.GetFPGAVersion(); + public static long FPGARevision => HalBase.GetFPGARevision(); + public static string SerialNumber => HalBase.GetSerialNumber(); + public static string Comments => HalBase.GetComments(); + public static int TeamNumber => HalBase.GetTeamNumber(); + public static long FPGATime => (long)HalBase.GetFPGATime(); + public static bool UserButton => HalBase.GetFPGAButton(); + public static ElectricPotential BatteryVoltage => HalPower.GetVinVoltage().Volts(); + public static bool SysActive => HalBase.GetSystemActive(); + public static bool BrownedOut => HalBase.GetBrownedOut(); + public static bool RSLState => HalBase.GetRSLState(); + public static bool IsSystemTimeValid => HalBase.GetSystemTimeValid(); + public static ElectricPotential InputVoltage => BatteryVoltage; + public static ElectricCurrent InputCurrent => HalPower.GetVinCurrent().Amperes(); } diff --git a/src/wpilibsharp/RobotRunner.cs b/src/wpilibsharp/RobotRunner.cs index d59a049e..375b6444 100644 --- a/src/wpilibsharp/RobotRunner.cs +++ b/src/wpilibsharp/RobotRunner.cs @@ -20,9 +20,6 @@ public static class RobotRunner public static void InitializeHAL() { - if (!HalBase.Initialize(500, 0)) - { - throw new HalInitializationException("Failed to initialize. Terminating"); - } + HalBase.Initialize(); } } diff --git a/src/wpilibsharp/Timer.cs b/src/wpilibsharp/Timer.cs new file mode 100644 index 00000000..bccf96da --- /dev/null +++ b/src/wpilibsharp/Timer.cs @@ -0,0 +1,89 @@ +using UnitsNet; +using UnitsNet.NumberExtensions.NumberToDuration; + +namespace WPILib; + +public sealed class Timer +{ + public static Duration FPGATimestamp => RobotController.FPGATime.Seconds(); + + public static Duration MatchTime => DriverStation.MatchTime.Seconds(); + + public static void Delay(Duration time) + { + Thread.Sleep((int)time.Milliseconds); + } + + private Duration m_startTime; + private Duration m_accumulatedTime; + private bool m_running; + + public Timer() + { + Reset(); + } + + public Duration Elapsed + { + get + { + if (m_running) + { + return m_accumulatedTime + (FPGATimestamp - m_startTime); + } + else + { + return m_accumulatedTime; + } + } + } + + public void Reset() + { + m_accumulatedTime = 0.Seconds(); + m_startTime = FPGATimestamp; + } + + public void Start() + { + if (!m_running) + { + m_startTime = FPGATimestamp; + m_running = true; + } + } + + public void Restart() + { + if (m_running) + { + Stop(); + } + Reset(); + Start(); + } + + public void Stop() + { + m_accumulatedTime = Elapsed; + m_running = false; + } + + public bool HasElapsed(Duration time) + { + return Elapsed >= time; + } + + public bool AdvanceIfElapsed(Duration time) + { + if (Elapsed >= time) + { + m_startTime += time; + return true; + } + else + { + return false; + } + } +} diff --git a/src/wpiutil/Concurrent/Synchronization.cs b/src/wpiutil/Concurrent/Synchronization.cs index f839dccc..d3acf0c1 100644 --- a/src/wpiutil/Concurrent/Synchronization.cs +++ b/src/wpiutil/Concurrent/Synchronization.cs @@ -1,5 +1,6 @@ using System.Runtime.InteropServices.Marshalling; using CommunityToolkit.Diagnostics; +using UnitsNet; using WPIUtil.Handles; using WPIUtil.Natives; @@ -9,14 +10,34 @@ namespace WPIUtil.Concurrent; public static class Synchronization { + public static SynchronizationResult WaitForObject(WpiEvent handle) + { + return WaitForObject(handle.Handle); + } + + public static SynchronizationResult WaitForObject(T handle) where T : IWpiSynchronizationHandle + { + return WaitForObject(handle.Handle); + } + public static SynchronizationResult WaitForObject(int handle) { return SynchronizationNative.WaitForObject(handle) ? SynchronizationResult.Signaled : SynchronizationResult.Cancelled; } - public static SynchronizationResult WaitForObject(int handle, TimeSpan timeout) + public static SynchronizationResult WaitForObject(WpiEvent handle, Duration timeout) + { + return WaitForObject(handle.Handle, timeout); + } + + public static SynchronizationResult WaitForObject(T handle, Duration timeout) where T : IWpiSynchronizationHandle + { + return WaitForObject(handle.Handle, timeout); + } + + public static SynchronizationResult WaitForObject(int handle, Duration timeout) { - bool signaled = SynchronizationNative.WaitForObjectTimeout(handle, timeout.TotalSeconds, out var timedOut); + bool signaled = SynchronizationNative.WaitForObjectTimeout(handle, timeout.Seconds, out var timedOut); if (signaled) { return SynchronizationResult.Signaled; @@ -42,14 +63,14 @@ public static ReadOnlySpan WaitForObjects(ReadOnlySpan handles, Span WaitForObjects(ReadOnlySpan handles, Span signaled, TimeSpan timeout, out bool timedOut) + public static ReadOnlySpan WaitForObjects(ReadOnlySpan handles, Span signaled, Duration timeout, out bool timedOut) { if (signaled.Length < handles.Length) { ThrowHelper.ThrowArgumentOutOfRangeException($"{nameof(signaled)} must have at least as much capacity as {nameof(handles)}"); } - int written = SynchronizationNative.WaitForObjectsTimeout(handles, handles.Length, signaled, timeout.TotalSeconds, out timedOut); + int written = SynchronizationNative.WaitForObjectsTimeout(handles, handles.Length, signaled, timeout.Seconds, out timedOut); return signaled[..written]; } }