Skip to content

Commit ddb931d

Browse files
ThadHouseThadHouse
committed
Add EventLoop based classes
1 parent 2933cca commit ddb931d

11 files changed

+459
-1
lines changed

‎src/wpilibsharp/Event/BooleanEvent.cs

+94
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,94 @@
1+
using CommunityToolkit.Diagnostics;
2+
using UnitsNet;
3+
using WPIMath.Filter;
4+
using WPIUtil;
5+
using WPIUtil.Atomic;
6+
7+
namespace WPILib.Event;
8+
9+
public class BooleanEvent
10+
{
11+
protected EventLoop Loop { get; }
12+
13+
private readonly Func<bool> m_signal;
14+
private readonly AtomicBool m_state = new(false);
15+
16+
public BooleanEvent(EventLoop loop, Func<bool> signal)
17+
{
18+
Loop = WpiGuard.RequireNotNull(loop);
19+
m_signal = WpiGuard.RequireNotNull(signal);
20+
m_state.Set(m_signal());
21+
loop.Bind(() => m_state.Set(m_signal()));
22+
}
23+
24+
public bool Get()
25+
{
26+
return m_state.Get();
27+
}
28+
29+
public void IfHigh(Action action)
30+
{
31+
Loop.Bind(() =>
32+
{
33+
if (m_state.Get())
34+
{
35+
action();
36+
}
37+
});
38+
}
39+
40+
public BooleanEvent Rising()
41+
{
42+
bool previous = m_state.Get();
43+
return new BooleanEvent(Loop, () =>
44+
{
45+
bool present = m_state.Get();
46+
bool ret = !previous && present;
47+
previous = present;
48+
return ret;
49+
});
50+
}
51+
52+
public BooleanEvent Falling()
53+
{
54+
bool previous = m_state.Get();
55+
return new BooleanEvent(Loop, () =>
56+
{
57+
bool present = m_state.Get();
58+
bool ret = previous && !present;
59+
previous = present;
60+
return ret;
61+
});
62+
}
63+
64+
public BooleanEvent Debounce(Duration duration, Debouncer.DebounceType type = Debouncer.DebounceType.Rising)
65+
{
66+
Debouncer debouncer = new(duration, type);
67+
return new BooleanEvent(Loop, () =>
68+
{
69+
return debouncer.Calculate(m_state.Get());
70+
});
71+
}
72+
73+
public BooleanEvent Negate()
74+
{
75+
return new BooleanEvent(Loop, () => !m_state.Get());
76+
}
77+
78+
public BooleanEvent And(Func<bool> other)
79+
{
80+
Guard.IsNotNull(other);
81+
return new BooleanEvent(Loop, () => m_state.Get() && other());
82+
}
83+
84+
public BooleanEvent Or(Func<bool> other)
85+
{
86+
Guard.IsNotNull(other);
87+
return new BooleanEvent(Loop, () => m_state.Get() || other());
88+
}
89+
90+
public T CastTo<T>(Func<EventLoop, Func<bool>, T> ctor)
91+
{
92+
return ctor(Loop, m_state.Get);
93+
}
94+
}

‎src/wpilibsharp/Event/EventLoop.cs

+46
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
using CommunityToolkit.Diagnostics;
2+
3+
namespace WPILib.Event;
4+
5+
public sealed class EventLoop
6+
{
7+
private readonly List<Action> m_bindings = [];
8+
9+
private bool m_running;
10+
11+
public EventLoop() { }
12+
13+
public void Bind(Action action)
14+
{
15+
if (m_running)
16+
{
17+
ThrowHelper.ThrowInvalidOperationException("Cannot bind EventLoop while it is running");
18+
}
19+
m_bindings.Add(action);
20+
}
21+
22+
public void Pool()
23+
{
24+
try
25+
{
26+
m_running = true;
27+
foreach (var action in m_bindings)
28+
{
29+
action();
30+
}
31+
}
32+
finally
33+
{
34+
m_running = false;
35+
}
36+
}
37+
38+
public void Clear()
39+
{
40+
if (m_running)
41+
{
42+
ThrowHelper.ThrowInvalidOperationException("Cannot bind EventLoop while it is running");
43+
}
44+
m_bindings.Clear();
45+
}
46+
}

‎src/wpilibsharp/Event/NetworkBooleanEvent.cs

+32
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
using CommunityToolkit.Diagnostics;
2+
using NetworkTables;
3+
4+
namespace WPILib.Event;
5+
6+
public class NetworkBooleanEvent : BooleanEvent
7+
{
8+
public NetworkBooleanEvent(EventLoop loop, BooleanTopic topic) : this(loop, topic.Subscribe(false))
9+
{
10+
11+
}
12+
13+
public NetworkBooleanEvent(EventLoop loop, IBooleanSubscriber sub) : base(loop, () => sub.Topic.Instance.Connected && sub.Get())
14+
{
15+
Guard.IsNotNull(sub);
16+
}
17+
18+
public NetworkBooleanEvent(EventLoop loop, NetworkTable table, string topicName) : this(loop, table.GetBooleanTopic(topicName))
19+
{
20+
21+
}
22+
23+
public NetworkBooleanEvent(EventLoop loop, string tableName, string topicName) : this(loop, NetworkTableInstance.Default, tableName, topicName)
24+
{
25+
26+
}
27+
28+
public NetworkBooleanEvent(EventLoop loop, NetworkTableInstance inst, string tableName, string topicName) : this(loop, inst.GetTable(tableName), topicName)
29+
{
30+
31+
}
32+
}

‎src/wpilibsharp/TimedRobot.cs

+1-1
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@ namespace WPILib;
77

88
public class TimedRobot : IterativeRobotBase
99
{
10-
private class Callback(Action func, Duration startTime, Duration period, Duration offset)
10+
private sealed class Callback(Action func, Duration startTime, Duration period, Duration offset)
1111
{
1212
public Action CB { get; } = func;
1313
public Duration Period { get; } = period;

‎src/wpimath/Filter/Debouncer.cs

+71
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,71 @@
1+
using CommunityToolkit.Diagnostics;
2+
using UnitsNet;
3+
4+
namespace WPIMath.Filter;
5+
6+
public class Debouncer
7+
{
8+
public enum DebounceType
9+
{
10+
Rising,
11+
Falling,
12+
Both
13+
}
14+
15+
private readonly Duration m_debounceTime;
16+
private readonly DebounceType m_debounceType;
17+
private bool m_baseline;
18+
19+
private Duration m_prevTime;
20+
21+
public Debouncer(Duration debounceTime, DebounceType type = DebounceType.Rising)
22+
{
23+
m_debounceTime = debounceTime;
24+
m_debounceType = type;
25+
26+
ResetTimer();
27+
28+
switch (m_debounceType)
29+
{
30+
case DebounceType.Both:
31+
case DebounceType.Rising:
32+
m_baseline = false;
33+
break;
34+
case DebounceType.Falling:
35+
m_baseline = true;
36+
break;
37+
default:
38+
ThrowHelper.ThrowArgumentOutOfRangeException(nameof(type));
39+
break;
40+
}
41+
}
42+
43+
private void ResetTimer()
44+
{
45+
m_prevTime = MathSharedStore.Timestamp;
46+
}
47+
48+
private bool Elapsed => MathSharedStore.Timestamp - m_prevTime >= m_debounceTime;
49+
50+
public bool Calculate(bool input)
51+
{
52+
if (input == m_baseline)
53+
{
54+
ResetTimer();
55+
}
56+
57+
if (Elapsed)
58+
{
59+
if (m_debounceType == DebounceType.Both)
60+
{
61+
m_baseline = input;
62+
ResetTimer();
63+
}
64+
return input;
65+
}
66+
else
67+
{
68+
return m_baseline;
69+
}
70+
}
71+
}

‎src/wpimath/IMathShared.cs

+10
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
using UnitsNet;
2+
3+
namespace WPIMath;
4+
5+
public interface IMathShared
6+
{
7+
void ReportError(string error, string stackTrace);
8+
void ReportUsage(MathUsageId id, int count);
9+
Duration Timestamp { get; }
10+
}

‎src/wpimath/MathSharedStore.cs

+58
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
using UnitsNet;
2+
using UnitsNet.NumberExtensions.NumberToDuration;
3+
using WPIUtil.Natives;
4+
5+
namespace WPIMath;
6+
7+
public static class MathSharedStore
8+
{
9+
private static IMathShared? m_mathShared;
10+
private static readonly object m_lockObject = new();
11+
12+
private sealed class DefaultMathShared : IMathShared
13+
{
14+
public Duration Timestamp => TimestampNative.Now().Microseconds();
15+
16+
public void ReportError(string error, string stackTrace)
17+
{
18+
}
19+
20+
public void ReportUsage(MathUsageId id, int count)
21+
{
22+
}
23+
}
24+
25+
public static IMathShared MathShared
26+
{
27+
get
28+
{
29+
lock (m_lockObject)
30+
{
31+
if (m_mathShared == null)
32+
{
33+
m_mathShared = new DefaultMathShared();
34+
}
35+
return m_mathShared;
36+
}
37+
}
38+
set
39+
{
40+
lock (m_lockObject)
41+
{
42+
m_mathShared = value;
43+
}
44+
}
45+
}
46+
47+
public static void ReportError(string error, string stackTrace)
48+
{
49+
MathShared.ReportError(error, stackTrace);
50+
}
51+
52+
public static void ReportUsage(MathUsageId id, int count)
53+
{
54+
MathShared.ReportUsage(id, count);
55+
}
56+
57+
public static Duration Timestamp => MathShared.Timestamp;
58+
}

‎src/wpimath/MathUsageId.cs

+34
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
namespace WPIMath;
2+
3+
public enum MathUsageId
4+
{
5+
/** DifferentialDriveKinematics. */
6+
KinematicsDifferentialDrive,
7+
8+
/** MecanumDriveKinematics. */
9+
KinematicsMecanumDrive,
10+
11+
/** SwerveDriveKinematics. */
12+
KinematicsSwerveDrive,
13+
14+
/** TrapezoidProfile. */
15+
TrajectoryTrapezoidProfile,
16+
17+
/** LinearFilter. */
18+
FilterLinear,
19+
20+
/** DifferentialDriveOdometry. */
21+
OdometryDifferentialDrive,
22+
23+
/** SwerveDriveOdometry. */
24+
OdometrySwerveDrive,
25+
26+
/** MecanumDriveOdometry. */
27+
OdometryMecanumDrive,
28+
29+
/** PIDController. */
30+
ControllerPIDController2,
31+
32+
/** ProfiledPIDController. */
33+
ControllerProfiledPIDController,
34+
}

‎src/wpiutil/Atomic/AtomicBool.cs

+36
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
using System.Runtime.CompilerServices;
2+
3+
namespace WPIUtil.Atomic;
4+
5+
public sealed class AtomicBool
6+
{
7+
private int m_value;
8+
9+
public AtomicBool()
10+
{
11+
m_value = 0;
12+
}
13+
14+
public AtomicBool(bool initialValue)
15+
{
16+
m_value = initialValue ? 1 : 0;
17+
}
18+
19+
[MethodImpl(MethodImplOptions.AggressiveInlining)]
20+
public bool Get()
21+
{
22+
return Interlocked.CompareExchange(ref m_value, 0, 0) != 0;
23+
}
24+
25+
[MethodImpl(MethodImplOptions.AggressiveInlining)]
26+
public bool GetAndSet(bool newValue)
27+
{
28+
return Interlocked.Exchange(ref m_value, newValue ? 1 : 0) != 0;
29+
}
30+
31+
[MethodImpl(MethodImplOptions.AggressiveInlining)]
32+
public void Set(bool newValue)
33+
{
34+
Interlocked.Exchange(ref m_value, newValue ? 1 : 0);
35+
}
36+
}

0 commit comments

Comments
 (0)