diff --git a/src/wpilibsharp/Event/BooleanEvent.cs b/src/wpilibsharp/Event/BooleanEvent.cs new file mode 100644 index 00000000..e135817a --- /dev/null +++ b/src/wpilibsharp/Event/BooleanEvent.cs @@ -0,0 +1,94 @@ +using CommunityToolkit.Diagnostics; +using UnitsNet; +using WPIMath.Filter; +using WPIUtil; +using WPIUtil.Atomic; + +namespace WPILib.Event; + +public class BooleanEvent +{ + protected EventLoop Loop { get; } + + private readonly Func m_signal; + private readonly AtomicBool m_state = new(false); + + public BooleanEvent(EventLoop loop, Func signal) + { + Loop = WpiGuard.RequireNotNull(loop); + m_signal = WpiGuard.RequireNotNull(signal); + m_state.Set(m_signal()); + loop.Bind(() => m_state.Set(m_signal())); + } + + public bool Get() + { + return m_state.Get(); + } + + public void IfHigh(Action action) + { + Loop.Bind(() => + { + if (m_state.Get()) + { + action(); + } + }); + } + + public BooleanEvent Rising() + { + bool previous = m_state.Get(); + return new BooleanEvent(Loop, () => + { + bool present = m_state.Get(); + bool ret = !previous && present; + previous = present; + return ret; + }); + } + + public BooleanEvent Falling() + { + bool previous = m_state.Get(); + return new BooleanEvent(Loop, () => + { + bool present = m_state.Get(); + bool ret = previous && !present; + previous = present; + return ret; + }); + } + + public BooleanEvent Debounce(Duration duration, Debouncer.DebounceType type = Debouncer.DebounceType.Rising) + { + Debouncer debouncer = new(duration, type); + return new BooleanEvent(Loop, () => + { + return debouncer.Calculate(m_state.Get()); + }); + } + + public BooleanEvent Negate() + { + return new BooleanEvent(Loop, () => !m_state.Get()); + } + + public BooleanEvent And(Func other) + { + Guard.IsNotNull(other); + return new BooleanEvent(Loop, () => m_state.Get() && other()); + } + + public BooleanEvent Or(Func other) + { + Guard.IsNotNull(other); + return new BooleanEvent(Loop, () => m_state.Get() || other()); + } + + public T CastTo(Func, T> ctor) + { + return ctor(Loop, m_state.Get); + } +} diff --git a/src/wpilibsharp/Event/EventLoop.cs b/src/wpilibsharp/Event/EventLoop.cs new file mode 100644 index 00000000..5f96bf6f --- /dev/null +++ b/src/wpilibsharp/Event/EventLoop.cs @@ -0,0 +1,46 @@ +using CommunityToolkit.Diagnostics; + +namespace WPILib.Event; + +public sealed class EventLoop +{ + private readonly List m_bindings = []; + + private bool m_running; + + public EventLoop() { } + + public void Bind(Action action) + { + if (m_running) + { + ThrowHelper.ThrowInvalidOperationException("Cannot bind EventLoop while it is running"); + } + m_bindings.Add(action); + } + + public void Pool() + { + try + { + m_running = true; + foreach (var action in m_bindings) + { + action(); + } + } + finally + { + m_running = false; + } + } + + public void Clear() + { + if (m_running) + { + ThrowHelper.ThrowInvalidOperationException("Cannot bind EventLoop while it is running"); + } + m_bindings.Clear(); + } +} diff --git a/src/wpilibsharp/Event/NetworkBooleanEvent.cs b/src/wpilibsharp/Event/NetworkBooleanEvent.cs new file mode 100644 index 00000000..f0cbd464 --- /dev/null +++ b/src/wpilibsharp/Event/NetworkBooleanEvent.cs @@ -0,0 +1,32 @@ +using CommunityToolkit.Diagnostics; +using NetworkTables; + +namespace WPILib.Event; + +public class NetworkBooleanEvent : BooleanEvent +{ + public NetworkBooleanEvent(EventLoop loop, BooleanTopic topic) : this(loop, topic.Subscribe(false)) + { + + } + + public NetworkBooleanEvent(EventLoop loop, IBooleanSubscriber sub) : base(loop, () => sub.Topic.Instance.Connected && sub.Get()) + { + Guard.IsNotNull(sub); + } + + public NetworkBooleanEvent(EventLoop loop, NetworkTable table, string topicName) : this(loop, table.GetBooleanTopic(topicName)) + { + + } + + public NetworkBooleanEvent(EventLoop loop, string tableName, string topicName) : this(loop, NetworkTableInstance.Default, tableName, topicName) + { + + } + + public NetworkBooleanEvent(EventLoop loop, NetworkTableInstance inst, string tableName, string topicName) : this(loop, inst.GetTable(tableName), topicName) + { + + } +} diff --git a/src/wpilibsharp/TimedRobot.cs b/src/wpilibsharp/TimedRobot.cs index 11383384..b144b0e7 100644 --- a/src/wpilibsharp/TimedRobot.cs +++ b/src/wpilibsharp/TimedRobot.cs @@ -7,7 +7,7 @@ namespace WPILib; public class TimedRobot : IterativeRobotBase { - private class Callback(Action func, Duration startTime, Duration period, Duration offset) + private sealed class Callback(Action func, Duration startTime, Duration period, Duration offset) { public Action CB { get; } = func; public Duration Period { get; } = period; diff --git a/src/wpimath/Filter/Debouncer.cs b/src/wpimath/Filter/Debouncer.cs new file mode 100644 index 00000000..25f7c7cc --- /dev/null +++ b/src/wpimath/Filter/Debouncer.cs @@ -0,0 +1,71 @@ +using CommunityToolkit.Diagnostics; +using UnitsNet; + +namespace WPIMath.Filter; + +public class Debouncer +{ + public enum DebounceType + { + Rising, + Falling, + Both + } + + private readonly Duration m_debounceTime; + private readonly DebounceType m_debounceType; + private bool m_baseline; + + private Duration m_prevTime; + + public Debouncer(Duration debounceTime, DebounceType type = DebounceType.Rising) + { + m_debounceTime = debounceTime; + m_debounceType = type; + + ResetTimer(); + + switch (m_debounceType) + { + case DebounceType.Both: + case DebounceType.Rising: + m_baseline = false; + break; + case DebounceType.Falling: + m_baseline = true; + break; + default: + ThrowHelper.ThrowArgumentOutOfRangeException(nameof(type)); + break; + } + } + + private void ResetTimer() + { + m_prevTime = MathSharedStore.Timestamp; + } + + private bool Elapsed => MathSharedStore.Timestamp - m_prevTime >= m_debounceTime; + + public bool Calculate(bool input) + { + if (input == m_baseline) + { + ResetTimer(); + } + + if (Elapsed) + { + if (m_debounceType == DebounceType.Both) + { + m_baseline = input; + ResetTimer(); + } + return input; + } + else + { + return m_baseline; + } + } +} diff --git a/src/wpimath/IMathShared.cs b/src/wpimath/IMathShared.cs new file mode 100644 index 00000000..60deffe1 --- /dev/null +++ b/src/wpimath/IMathShared.cs @@ -0,0 +1,10 @@ +using UnitsNet; + +namespace WPIMath; + +public interface IMathShared +{ + void ReportError(string error, string stackTrace); + void ReportUsage(MathUsageId id, int count); + Duration Timestamp { get; } +} diff --git a/src/wpimath/MathSharedStore.cs b/src/wpimath/MathSharedStore.cs new file mode 100644 index 00000000..a24b65d5 --- /dev/null +++ b/src/wpimath/MathSharedStore.cs @@ -0,0 +1,58 @@ +using UnitsNet; +using UnitsNet.NumberExtensions.NumberToDuration; +using WPIUtil.Natives; + +namespace WPIMath; + +public static class MathSharedStore +{ + private static IMathShared? m_mathShared; + private static readonly object m_lockObject = new(); + + private sealed class DefaultMathShared : IMathShared + { + public Duration Timestamp => TimestampNative.Now().Microseconds(); + + public void ReportError(string error, string stackTrace) + { + } + + public void ReportUsage(MathUsageId id, int count) + { + } + } + + public static IMathShared MathShared + { + get + { + lock (m_lockObject) + { + if (m_mathShared == null) + { + m_mathShared = new DefaultMathShared(); + } + return m_mathShared; + } + } + set + { + lock (m_lockObject) + { + m_mathShared = value; + } + } + } + + public static void ReportError(string error, string stackTrace) + { + MathShared.ReportError(error, stackTrace); + } + + public static void ReportUsage(MathUsageId id, int count) + { + MathShared.ReportUsage(id, count); + } + + public static Duration Timestamp => MathShared.Timestamp; +} diff --git a/src/wpimath/MathUsageId.cs b/src/wpimath/MathUsageId.cs new file mode 100644 index 00000000..2b6ee7de --- /dev/null +++ b/src/wpimath/MathUsageId.cs @@ -0,0 +1,34 @@ +namespace WPIMath; + +public enum MathUsageId +{ + /** DifferentialDriveKinematics. */ + KinematicsDifferentialDrive, + + /** MecanumDriveKinematics. */ + KinematicsMecanumDrive, + + /** SwerveDriveKinematics. */ + KinematicsSwerveDrive, + + /** TrapezoidProfile. */ + TrajectoryTrapezoidProfile, + + /** LinearFilter. */ + FilterLinear, + + /** DifferentialDriveOdometry. */ + OdometryDifferentialDrive, + + /** SwerveDriveOdometry. */ + OdometrySwerveDrive, + + /** MecanumDriveOdometry. */ + OdometryMecanumDrive, + + /** PIDController. */ + ControllerPIDController2, + + /** ProfiledPIDController. */ + ControllerProfiledPIDController, +} diff --git a/src/wpiutil/Atomic/AtomicBool.cs b/src/wpiutil/Atomic/AtomicBool.cs new file mode 100644 index 00000000..1f8f7d63 --- /dev/null +++ b/src/wpiutil/Atomic/AtomicBool.cs @@ -0,0 +1,36 @@ +using System.Runtime.CompilerServices; + +namespace WPIUtil.Atomic; + +public sealed class AtomicBool +{ + private int m_value; + + public AtomicBool() + { + m_value = 0; + } + + public AtomicBool(bool initialValue) + { + m_value = initialValue ? 1 : 0; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool Get() + { + return Interlocked.CompareExchange(ref m_value, 0, 0) != 0; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public bool GetAndSet(bool newValue) + { + return Interlocked.Exchange(ref m_value, newValue ? 1 : 0) != 0; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Set(bool newValue) + { + Interlocked.Exchange(ref m_value, newValue ? 1 : 0); + } +} diff --git a/src/wpiutil/Atomic/AtomicInteger.cs b/src/wpiutil/Atomic/AtomicInteger.cs new file mode 100644 index 00000000..81179b66 --- /dev/null +++ b/src/wpiutil/Atomic/AtomicInteger.cs @@ -0,0 +1,36 @@ +using System.Runtime.CompilerServices; + +namespace WPIUtil.Atomic; + +public sealed class AtomicInteger +{ + private int m_value; + + public AtomicInteger() + { + m_value = 0; + } + + public AtomicInteger(int initialValue) + { + m_value = initialValue; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public int Get() + { + return Interlocked.CompareExchange(ref m_value, 0, 0); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public int GetAndSet(int newValue) + { + return Interlocked.Exchange(ref m_value, newValue); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Set(int newValue) + { + Interlocked.Exchange(ref m_value, newValue); + } +} diff --git a/src/wpiutil/Atomic/AtomicWpiHandle.cs b/src/wpiutil/Atomic/AtomicWpiHandle.cs new file mode 100644 index 00000000..06bd7e46 --- /dev/null +++ b/src/wpiutil/Atomic/AtomicWpiHandle.cs @@ -0,0 +1,41 @@ +using System.Runtime.CompilerServices; +using WPIUtil.Handles; + +namespace WPIUtil.Atomic; + +public sealed class AtomicWpiHandle where T : struct, IWPIIntHandle +{ + private int m_value; + + public AtomicWpiHandle() + { + m_value = 0; + } + + public AtomicWpiHandle(T initialValue) + { + m_value = initialValue.Handle; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public T Get() + { + T ret = default; + ret.Handle = Interlocked.CompareExchange(ref m_value, 0, 0); + return ret; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public T GetAndSet(T newValue) + { + T ret = default; + ret.Handle = Interlocked.Exchange(ref m_value, newValue.Handle); + return ret; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void Set(T newValue) + { + Interlocked.Exchange(ref m_value, newValue.Handle); + } +}