diff --git a/WPILib.sln b/WPILib.sln index b1d72f1b..ea9e2778 100644 --- a/WPILib.sln +++ b/WPILib.sln @@ -29,6 +29,8 @@ Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "wpimath.test", "test\wpimat EndProject Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "wpiutil.test", "test\wpiutil.test\wpiutil.test.csproj", "{4EF1913A-C495-491D-9783-D758E4723170}" EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "wpilibsharp", "src\wpilibsharp\wpilibsharp.csproj", "{AA0BE2D8-DCE2-415C-A60B-4382F50D8C91}" +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|Any CPU = Debug|Any CPU @@ -78,6 +80,10 @@ Global {4EF1913A-C495-491D-9783-D758E4723170}.Debug|Any CPU.Build.0 = Debug|Any CPU {4EF1913A-C495-491D-9783-D758E4723170}.Release|Any CPU.ActiveCfg = Release|Any CPU {4EF1913A-C495-491D-9783-D758E4723170}.Release|Any CPU.Build.0 = Release|Any CPU + {AA0BE2D8-DCE2-415C-A60B-4382F50D8C91}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {AA0BE2D8-DCE2-415C-A60B-4382F50D8C91}.Debug|Any CPU.Build.0 = Debug|Any CPU + {AA0BE2D8-DCE2-415C-A60B-4382F50D8C91}.Release|Any CPU.ActiveCfg = Release|Any CPU + {AA0BE2D8-DCE2-415C-A60B-4382F50D8C91}.Release|Any CPU.Build.0 = Release|Any CPU EndGlobalSection GlobalSection(NestedProjects) = preSolution {8F38C25E-641E-47FC-AC0A-0717F2159E8F} = {DB664556-4BF0-4874-8CB6-DC24E60A67AF} @@ -90,5 +96,6 @@ Global {602CB8AB-FB5A-4DF7-B757-292789DE22CF} = {DB664556-4BF0-4874-8CB6-DC24E60A67AF} {FA3D5F6A-561C-4F12-AE91-B10AE11F7BB8} = {AD95ECD8-E708-4FB4-9B7E-A8A8EF3FCB3E} {4EF1913A-C495-491D-9783-D758E4723170} = {AD95ECD8-E708-4FB4-9B7E-A8A8EF3FCB3E} + {AA0BE2D8-DCE2-415C-A60B-4382F50D8C91} = {DB664556-4BF0-4874-8CB6-DC24E60A67AF} EndGlobalSection EndGlobal diff --git a/src/wpilibsharp/AddressableLED.cs b/src/wpilibsharp/AddressableLED.cs deleted file mode 100644 index 5eeac9cf..00000000 --- a/src/wpilibsharp/AddressableLED.cs +++ /dev/null @@ -1,67 +0,0 @@ -using System; -using Hal; - -namespace WPILib -{ - public sealed class AddressableLED : IDisposable - { - - - private readonly int m_pwmHandle; - private readonly int m_handle; - - public AddressableLED(int port) - { - m_pwmHandle = Hal.PWMLowLevel.InitializePort(Hal.HALLowLevel.GetPort(port)); - m_handle = Hal.AddressableLEDLowLevel.Initialize(m_pwmHandle); - UsageReporting.Report(ResourceType.AddressableLEDs, port + 1); - } - - public void Dispose() - { - if (m_handle != 0) - { - Hal.AddressableLEDLowLevel.Free(m_handle); - } - if (m_pwmHandle != 0) - { - Hal.PWMLowLevel.FreePort(m_pwmHandle); - } - } - - public void SetLength(int length) - { - Hal.AddressableLEDLowLevel.SetLength(m_handle, length); - } - - public void SetData(ReadOnlySpan data) - { - Hal.AddressableLEDLowLevel.WriteData(m_handle, data); - } - - public unsafe void SetData(AddressableLEDData* data, int length) - { - Hal.AddressableLEDLowLevel.WriteData(m_handle, data, length); - } - - public void SetBitTiming(int lowTime0NanoSeconds, int highTime0NanoSeconds, int lowTime1NanoSeconds, int highTime1NanoSeconds) - { - Hal.AddressableLEDLowLevel.SetBitTiming(m_handle, lowTime0NanoSeconds, highTime0NanoSeconds, lowTime1NanoSeconds, highTime1NanoSeconds); - } - - public void SetSyncTime(int syncTimeMicroSeconds) - { - Hal.AddressableLEDLowLevel.SetSyncTime(m_handle, syncTimeMicroSeconds); - } - - public void Start() - { - Hal.AddressableLEDLowLevel.StartOutput(m_handle); - } - - public void Stop() - { - Hal.AddressableLEDLowLevel.StopOutput(m_handle); - } - } -} diff --git a/src/wpilibsharp/AnalogAccelerometer.cs b/src/wpilibsharp/AnalogAccelerometer.cs deleted file mode 100644 index b6c16d2f..00000000 --- a/src/wpilibsharp/AnalogAccelerometer.cs +++ /dev/null @@ -1,56 +0,0 @@ -using UnitsNet; -using System; -using WPILib.SmartDashboardNS; -using Hal; - -namespace WPILib -{ - public sealed class AnalogAccelerometer : ISendable, IDisposable - { - private readonly AnalogInput m_analogInput; - private double m_voltsPerG = 1.0; - private ElectricPotential m_zeroGVoltage = ElectricPotential.FromVolts(2.5); - - private readonly bool m_allocatedChannel; - - public AnalogAccelerometer(int channel) - : this(new AnalogInput(channel)) - { - m_allocatedChannel = true; - } - - public AnalogAccelerometer(AnalogInput analogInput) - { - m_analogInput = analogInput ?? throw new ArgumentNullException(nameof(analogInput)); - UsageReporting.Report(ResourceType.Accelerometer, analogInput.Channel + 1); - SendableRegistry.Instance.AddLW(this, "Accelerometer", analogInput.Channel); - } - - public void Dispose() - { - SendableRegistry.Instance.Remove(this); - if (m_allocatedChannel) - { - m_analogInput.Dispose(); - } - } - - public Acceleration Acceleration => Acceleration.FromStandardGravity((m_analogInput.AverageVoltage - m_zeroGVoltage).Volts / m_voltsPerG); - - public double Sensitivity - { - set => m_voltsPerG = value; - } - - public ElectricPotential Zero - { - set => m_zeroGVoltage = value; - } - - void ISendable.InitSendable(ISendableBuilder builder) - { - builder.SmartDashboardType = "Accelerometer"; - builder.AddDoubleProperty("Value", () => Acceleration.StandardGravity, null); - } - } -} diff --git a/src/wpilibsharp/AnalogAccumulator.cs b/src/wpilibsharp/AnalogAccumulator.cs deleted file mode 100644 index 1eb4ccaa..00000000 --- a/src/wpilibsharp/AnalogAccumulator.cs +++ /dev/null @@ -1,65 +0,0 @@ -using System; -using System.Threading; - -namespace WPILib -{ - public class AnalogAccumulator - { - private readonly AnalogInput m_analogInput; - private readonly int m_port; - private long m_accumulatorOffset; - - public AnalogAccumulator(AnalogInput analogInput) - { - m_analogInput = analogInput ?? throw new ArgumentNullException(nameof(analogInput)); - if (!analogInput.IsAccumulatorChannel) - { - throw new ArgumentException("Analog Input must be an accumulator channel", nameof(analogInput)); - } - m_port = analogInput.m_port; - - m_accumulatorOffset = 0; - Hal.AnalogAccumulatorLowLevel.InitAccumulator(m_port); - } - - public long InitialValue - { - set => m_accumulatorOffset = value; - } - - public void Reset() - { - Hal.AnalogAccumulatorLowLevel.ResetAccumulator(m_port); - - // Wait until the next sample, so the next call to getAccumulator*() - // won't have old values. - double sampleTime = 1.0 / AnalogInput.GlobalSampleRate; - double overSamples = 1 << m_analogInput.OversampleBits; - double averageSamples = 1 << m_analogInput.AverageBits; - Thread.Sleep(TimeSpan.FromSeconds(sampleTime * overSamples * averageSamples)); - } - - public int Center - { - set => Hal.AnalogAccumulatorLowLevel.SetAccumulatorCenter(m_port, value); - } - - public int Deadband - { - set => Hal.AnalogAccumulatorLowLevel.SetAccumulatorDeadband(m_port, value); - } - - public long Value => Hal.AnalogAccumulatorLowLevel.GetAccumulatorValue(m_port); - - public long Count => Hal.AnalogAccumulatorLowLevel.GetAccumulatorCount(m_port); - - public AnalogAccumulatorOutput Output - { - get - { - Hal.AnalogAccumulatorLowLevel.GetAccumulatorOutput(m_port, out var value, out var count); - return new AnalogAccumulatorOutput(count, value + m_accumulatorOffset); - } - } - } -} diff --git a/src/wpilibsharp/AnalogAccumulatorOutput.cs b/src/wpilibsharp/AnalogAccumulatorOutput.cs deleted file mode 100644 index d87418fc..00000000 --- a/src/wpilibsharp/AnalogAccumulatorOutput.cs +++ /dev/null @@ -1,45 +0,0 @@ -using System; - -namespace WPILib -{ - public readonly struct AnalogAccumulatorOutput : IEquatable - { - public long Count { get; } - public long Value { get; } - - public AnalogAccumulatorOutput(long count, long value) - { - Count = count; - Value = value; - } - - public override bool Equals(object? obj) - { - return obj is AnalogAccumulatorOutput output && Equals(output); - } - - public bool Equals(AnalogAccumulatorOutput other) - { - return Count == other.Count && - Value == other.Value; - } - - public override int GetHashCode() - { - var hashCode = -1663980258; - hashCode = hashCode * -1521134295 + Count.GetHashCode(); - hashCode = hashCode * -1521134295 + Value.GetHashCode(); - return hashCode; - } - - public static bool operator ==(AnalogAccumulatorOutput left, AnalogAccumulatorOutput right) - { - return left.Equals(right); - } - - public static bool operator !=(AnalogAccumulatorOutput left, AnalogAccumulatorOutput right) - { - return !(left == right); - } - } -} diff --git a/src/wpilibsharp/AnalogGyro.cs b/src/wpilibsharp/AnalogGyro.cs deleted file mode 100644 index b61d5767..00000000 --- a/src/wpilibsharp/AnalogGyro.cs +++ /dev/null @@ -1,85 +0,0 @@ -using System; -using Hal; -using UnitsNet; -using WPILib.SmartDashboardNS; - -namespace WPILib -{ - public sealed class AnalogGyro : GyroBase - { - private const double DefaultVoltsPerDegreePerSecond = 0.007; - private readonly AnalogInput m_analogInput; - private readonly bool m_channelAllocated; - private readonly int m_gyroHandle; - - public AnalogGyro(int channel, int center = 0, double offset = 0) - : this(new AnalogInput(channel), center, offset) - { - m_channelAllocated = true; - SendableRegistry.Instance.AddChild(this, m_analogInput); - } - - public AnalogGyro(AnalogInput input, int center = 0, double offset = 0) - { - m_analogInput = input ?? throw new ArgumentNullException(nameof(input)); - - m_gyroHandle = Hal.AnalogGyroLowLevel.Initialize(input.m_port); - - Hal.AnalogGyroLowLevel.Setup(m_gyroHandle); - - UsageReporting.Report(ResourceType.Gyro, m_analogInput.Channel + 1); - SendableRegistry.Instance.AddLW(this, "AnalogGyro", m_analogInput.Channel); - - if (center != 0 || offset != 0) - { - Hal.AnalogGyroLowLevel.SetParameters(m_gyroHandle, DefaultVoltsPerDegreePerSecond, offset, center); - Reset(); - } - else - { - Calibrate(); - } - - - } - - public override Angle Angle => Angle.FromDegrees(Hal.AnalogGyroLowLevel.GetAngle(m_gyroHandle)); - - public override RotationalSpeed Rate => RotationalSpeed.FromDegreesPerSecond(Hal.AnalogGyroLowLevel.GetRate(m_gyroHandle)); - - public override void Calibrate() - { - Hal.AnalogGyroLowLevel.Calibrate(m_gyroHandle); - } - - public override void Dispose() - { - SendableRegistry.Instance.Remove(this); - if (m_channelAllocated) - { - m_analogInput.Dispose(); - } - - Hal.AnalogGyroLowLevel.Free(m_gyroHandle); - } - - public override void Reset() - { - Hal.AnalogGyroLowLevel.Reset(m_gyroHandle); - } - - public double Offset => Hal.AnalogGyroLowLevel.GetOffset(m_gyroHandle); - - public int Center => Hal.AnalogGyroLowLevel.GetCenter(m_gyroHandle); - - public double Sensitivity - { - set => Hal.AnalogGyroLowLevel.SetVoltsPerDegreePerSecond(m_gyroHandle, value); - } - - public ElectricPotential Deadband - { - set => Hal.AnalogGyroLowLevel.SetDeadband(m_gyroHandle, value.Volts); - } - } -} diff --git a/src/wpilibsharp/AnalogInput.cs b/src/wpilibsharp/AnalogInput.cs deleted file mode 100644 index 1fe12466..00000000 --- a/src/wpilibsharp/AnalogInput.cs +++ /dev/null @@ -1,71 +0,0 @@ -using System; -using Hal; -using UnitsNet; -using WPILib.SmartDashboardNS; - -namespace WPILib -{ - public sealed class AnalogInput : ISendable, IDisposable - { - internal readonly int m_port; - public int Channel { get; } - - public AnalogInput(int channel) - { - Channel = channel; - - m_port = Hal.AnalogInputLowLevel.InitializePort(Hal.HALLowLevel.GetPort(channel)); - UsageReporting.Report(ResourceType.AnalogOutput, channel + 1); - SendableRegistry.Instance.AddLW(this, "AnalogInput", channel); - } - - public void Dispose() - { - SendableRegistry.Instance.Remove(this); - Hal.AnalogInputLowLevel.FreePort(m_port); - } - - public int RawValue => Hal.AnalogInputLowLevel.GetAnalogValue(m_port); - - public int RawAverageValue => Hal.AnalogInputLowLevel.GetAnalogAverageValue(m_port); - - public ElectricPotential Voltage => ElectricPotential.FromVolts(Hal.AnalogInputLowLevel.GetAnalogVoltage(m_port)); - - public ElectricPotential AverageVoltage => ElectricPotential.FromVolts(Hal.AnalogInputLowLevel.GetAnalogAverageVoltage(m_port)); - - public long LSBWeight => Hal.AnalogInputLowLevel.GetAnalogLSBWeight(m_port); - - public int Offset => Hal.AnalogInputLowLevel.GetAnalogOffset(m_port); - - public int AverageBits - { - get => Hal.AnalogInputLowLevel.GetAnalogAverageBits(m_port); - set => Hal.AnalogInputLowLevel.SetAnalogAverageBits(m_port, value); - } - - public int OversampleBits - { - get => Hal.AnalogInputLowLevel.GetAnalogOversampleBits(m_port); - set => Hal.AnalogInputLowLevel.SetAnalogOversampleBits(m_port, value); - } - - public bool IsAccumulatorChannel => Hal.AnalogAccumulatorLowLevel.IsAccumulatorChannel(m_port); - - public static double GlobalSampleRate - { - get => Hal.AnalogInputLowLevel.GetAnalogSampleRate(); - set => Hal.AnalogInputLowLevel.SetAnalogSampleRate(value); - } - - public SimDevice SimDevice - { - set => Hal.AnalogInputLowLevel.SetSimDevice(m_port, value.NativeHandle); - } - - void ISendable.InitSendable(ISendableBuilder builder) - { - builder.SmartDashboardType = "Analog Input"; - builder.AddDoubleProperty("Value", () => AverageVoltage.Volts, null); - } - } -} diff --git a/src/wpilibsharp/AnalogOutput.cs b/src/wpilibsharp/AnalogOutput.cs deleted file mode 100644 index 9091c13b..00000000 --- a/src/wpilibsharp/AnalogOutput.cs +++ /dev/null @@ -1,39 +0,0 @@ -using System; -using Hal; -using UnitsNet; -using WPILib.SmartDashboardNS; - -namespace WPILib -{ - public sealed class AnalogOutput : ISendable, IDisposable - { - private readonly int m_port; - public int Channel { get; } - - public AnalogOutput(int channel) - { - Channel = channel; - m_port = Hal.AnalogOutputLowLevel.InitializePort(Hal.HALLowLevel.GetPort(channel)); - UsageReporting.Report(ResourceType.AnalogOutput, channel + 1); - SendableRegistry.Instance.AddLW(this, "AnalogOutput", channel); - } - - public void Dispose() - { - SendableRegistry.Instance.Remove(this); - Hal.AnalogOutputLowLevel.FreePort(m_port); - } - - public ElectricPotential Voltage - { - get => ElectricPotential.FromVolts(Hal.AnalogOutputLowLevel.Get(m_port)); - set => Hal.AnalogOutputLowLevel.Set(m_port, value.Volts); - } - - void ISendable.InitSendable(ISendableBuilder builder) - { - builder.SmartDashboardType = "Analog Output"; - builder.AddDoubleProperty("Value", () => Voltage.Volts, (v) => Voltage = ElectricPotential.FromVolts(v)); - } - } -} diff --git a/src/wpilibsharp/AnalogPotentiometer.cs b/src/wpilibsharp/AnalogPotentiometer.cs deleted file mode 100644 index 9e02f69b..00000000 --- a/src/wpilibsharp/AnalogPotentiometer.cs +++ /dev/null @@ -1,47 +0,0 @@ -using System; -using UnitsNet; -using WPILib.Interfaces; -using WPILib.SmartDashboardNS; - -namespace WPILib -{ - public sealed class AnalogPotentiometer : IPotentiometer, ISendable, IDisposable - { - private readonly AnalogInput m_analogInput; - private readonly bool m_allocatedChannel; - private readonly Angle m_fullRange; - private readonly Angle m_offset; - - public AnalogPotentiometer(int channel, Angle fullRange, Angle offset = default) - : this(new AnalogInput(channel), fullRange, offset) - { - m_allocatedChannel = true; - SendableRegistry.Instance.AddChild(this, m_analogInput); - } - - public AnalogPotentiometer(AnalogInput analogInput, Angle fullRange, Angle offset = default) - { - m_analogInput = analogInput; - m_fullRange = fullRange; - m_offset = offset; - } - - - - public Angle Angle => (m_analogInput.Voltage / RobotController.Voltage5V) * m_fullRange + m_offset; - - public void Dispose() - { - SendableRegistry.Instance.Remove(this); - if (m_allocatedChannel) - { - m_analogInput.Dispose(); - } - } - - void ISendable.InitSendable(ISendableBuilder builder) - { - (m_analogInput as ISendable).InitSendable(builder); - } - } -} diff --git a/src/wpilibsharp/AnalogTrigger.cs b/src/wpilibsharp/AnalogTrigger.cs deleted file mode 100644 index 1ac62a23..00000000 --- a/src/wpilibsharp/AnalogTrigger.cs +++ /dev/null @@ -1,114 +0,0 @@ -using System; -using Hal; -using UnitsNet; -using WPILib.SmartDashboardNS; - -namespace WPILib -{ - public sealed class AnalogTrigger : ISendable, IDisposable - { - internal readonly int m_port; - private readonly AnalogInput? m_analogInput; - private readonly DutyCycle? m_dutyCycle; - private readonly bool m_ownsAnalog; - - public AnalogTrigger(int channel) - : this(new AnalogInput(channel)) - { - m_ownsAnalog = true; - SendableRegistry.Instance.AddChild(this, m_analogInput!); - } - - public AnalogTrigger(AnalogInput analogInput) - { - m_analogInput = analogInput ?? throw new ArgumentNullException(nameof(analogInput)); - m_port = Hal.AnalogTriggerLowLevel.Initialize(analogInput.m_port); - - var index = Index; - - UsageReporting.Report(ResourceType.AnalogTrigger, index + 1); - SendableRegistry.Instance.AddLW(this, "AnalogTrigger", index); - } - - public AnalogTrigger(DutyCycle dutyCycle) - { - m_dutyCycle = dutyCycle ?? throw new ArgumentNullException(nameof(dutyCycle)); - - m_port = Hal.AnalogTriggerLowLevel.InitializeDutyCycle(dutyCycle.m_handle); - - var index = Index; - - UsageReporting.Report(ResourceType.AnalogTrigger, index + 1); - SendableRegistry.Instance.AddLW(this, "AnalogTrigger", index); - } - - public void SetLimitsRaw(int lower, int upper) - { - if (lower > upper) - { - throw new BoundaryException("Lower bound is greater then upper"); - } - - Hal.AnalogTriggerLowLevel.SetLimitsRaw(m_port, lower, upper); - } - - public void SetLimitsDutyCycle(double lower, double upper) - { - if (lower > upper) - { - throw new BoundaryException("Lower bound is greater then upper"); - } - - Hal.AnalogTriggerLowLevel.SetLimitsDutyCycle(m_port, lower, upper); - } - - public void SetLimitsVoltage(ElectricPotential lower, ElectricPotential upper) - { - if (lower > upper) - { - throw new BoundaryException("Lower bound is greater then upper"); - } - - Hal.AnalogTriggerLowLevel.SetLimitsVoltage(m_port, lower.Volts, upper.Volts); - } - - public bool Averaged - { - set => Hal.AnalogTriggerLowLevel.SetAveraged(m_port, value); - } - - public bool Filtered - { - set => Hal.AnalogTriggerLowLevel.SetFiltered(m_port, value); - } - - public int Index => Hal.AnalogTriggerLowLevel.GetFPGAIndex(m_port); - - public bool InWindow => Hal.AnalogTriggerLowLevel.GetInWindow(m_port); - - public bool TriggerState => Hal.AnalogTriggerLowLevel.GetTriggerState(m_port); - - public AnalogTriggerOutput CreateOutput(AnalogTriggerType type) - { - return new AnalogTriggerOutput(this, type); - } - - public void Dispose() - { - SendableRegistry.Instance.Remove(this); - Hal.AnalogTriggerLowLevel.Clean(m_port); - if (m_ownsAnalog) - { - m_analogInput?.Dispose(); - } - } - - void ISendable.InitSendable(ISendableBuilder builder) - { - if (m_ownsAnalog) - { - (m_analogInput as ISendable)?.InitSendable(builder); - } - } - } -} diff --git a/src/wpilibsharp/AnalogTriggerOutput.cs b/src/wpilibsharp/AnalogTriggerOutput.cs deleted file mode 100644 index 80da63d0..00000000 --- a/src/wpilibsharp/AnalogTriggerOutput.cs +++ /dev/null @@ -1,34 +0,0 @@ -using System; -using Hal; -using WPILib.SmartDashboardNS; - -namespace WPILib -{ - public class AnalogTriggerOutput : IDigitalSource, ISendable - { - - private readonly AnalogTrigger m_trigger; - - public AnalogTriggerOutput(AnalogTrigger trigger, AnalogTriggerType type) - { - m_trigger = trigger ?? throw new ArgumentNullException(nameof(trigger)); - AnalogTriggerTypeForRouting = type; - - UsageReporting.Report(ResourceType.AnalogTriggerOutput, trigger.Index + 1, (int)type + 1); - } - - public bool Output => Hal.AnalogTriggerLowLevel.GetOutput(m_trigger.m_port, AnalogTriggerTypeForRouting); - - public bool IsAnalogTrigger => true; - - public int Channel => m_trigger.Index; - - public AnalogTriggerType AnalogTriggerTypeForRouting { get; } - - public int PortHandleForRouting => m_trigger.m_port; - - void ISendable.InitSendable(ISendableBuilder builder) - { - } - } -} diff --git a/src/wpilibsharp/AsynchronousInterrupt.cs b/src/wpilibsharp/AsynchronousInterrupt.cs deleted file mode 100644 index 0f9b1dce..00000000 --- a/src/wpilibsharp/AsynchronousInterrupt.cs +++ /dev/null @@ -1,77 +0,0 @@ -using System; -using System.Threading; - -namespace WPILib -{ - public sealed class AsynchronousInterrupt : IDisposable - { - private readonly SynchronousInterrupt m_synchronousInterrupt; - - private readonly Action m_callback; - - private Thread? m_interruptThread; - private CancellationTokenSource m_threadRunSource = new CancellationTokenSource(); - - public AsynchronousInterrupt(IDigitalSource source, Action callback) - { - m_synchronousInterrupt = new SynchronousInterrupt(source); - m_callback = callback; - } - - private void ThreadMain() - { - while (!m_threadRunSource.IsCancellationRequested) - { - var result = m_synchronousInterrupt.WaitForInterrupt(TimeSpan.FromSeconds(1)); - if (m_threadRunSource.IsCancellationRequested) - { - break; - } - if (result == 0) - { - continue; - } - m_callback((result & EdgeConfiguration.kRisingEdge) != 0, - (result & EdgeConfiguration.kFallingEdge) != 0); - } - - } - - public void Enable() - { - if (m_interruptThread != null) return; - - m_threadRunSource = new CancellationTokenSource(); - m_interruptThread = new Thread(ThreadMain) - { - Name = $"Interrupt Thread", - IsBackground = true - }; - m_interruptThread.Start(); - } - - public void Disable() - { - m_threadRunSource.Cancel(); - m_synchronousInterrupt.WakeupWaitingInterrupt(); - m_interruptThread?.Join(); - m_interruptThread = null; - } - - public void SetInterruptEdges(bool risingEdge, bool fallingEdge) - { - m_synchronousInterrupt.SetInterruptEdges(risingEdge, fallingEdge); - } - - public TimeSpan RisingTimestamp => m_synchronousInterrupt.RisingTimestamp; - - public TimeSpan FallingTimestamp => m_synchronousInterrupt.FallingTimestamp; - - public void Dispose() - { - Disable(); - m_synchronousInterrupt.Dispose(); - m_threadRunSource.Dispose(); - } - } -} diff --git a/src/wpilibsharp/BoundaryException.cs b/src/wpilibsharp/BoundaryException.cs deleted file mode 100644 index 5e3ab362..00000000 --- a/src/wpilibsharp/BoundaryException.cs +++ /dev/null @@ -1,21 +0,0 @@ -using System; - -namespace WPILib -{ - public class BoundaryException : Exception - { - public BoundaryException(string message) - : base(message) - { - - } - - public BoundaryException() - { - } - - public BoundaryException(string message, Exception innerException) : base(message, innerException) - { - } - } -} diff --git a/src/wpilibsharp/BuiltInAccelerometer.cs b/src/wpilibsharp/BuiltInAccelerometer.cs deleted file mode 100644 index 2be4b6be..00000000 --- a/src/wpilibsharp/BuiltInAccelerometer.cs +++ /dev/null @@ -1,55 +0,0 @@ -using System; -using Hal; -using UnitsNet; -using WPILib.Interfaces; -using WPILib.SmartDashboardNS; - -namespace WPILib -{ - public sealed class BuiltInAccelerometer : IAccelerometer, ISendable, IDisposable - { - public BuiltInAccelerometer(AccelerometerRange range = AccelerometerRange.k8G) - { - Range = range; - } - - public AccelerometerRange Range - { - set - { - - switch (value) - { - case AccelerometerRange.k2G: - case AccelerometerRange.k4G: - case AccelerometerRange.k8G: - Hal.AccelerometerLowLevel.SetActive(false); - Hal.AccelerometerLowLevel.SetRange(value); - Hal.AccelerometerLowLevel.SetActive(true); - break; - default: - throw new ArgumentOutOfRangeException($"{value} range not supported (use k2G, k4G, or k8G"); - } - } - } - - public Acceleration X => Acceleration.FromStandardGravity(Hal.AccelerometerLowLevel.GetX()); - - public Acceleration Y => Acceleration.FromStandardGravity(Hal.AccelerometerLowLevel.GetY()); - - public Acceleration Z => Acceleration.FromStandardGravity(Hal.AccelerometerLowLevel.GetZ()); - - public void Dispose() - { - SendableRegistry.Instance.Remove(this); - } - - void ISendable.InitSendable(ISendableBuilder builder) - { - builder.SmartDashboardType = "3AxisAccelerometer"; - builder.AddDoubleProperty("X", () => X.StandardGravity, null); - builder.AddDoubleProperty("Y", () => Y.StandardGravity, null); - builder.AddDoubleProperty("Z", () => Z.StandardGravity, null); - } - } -} diff --git a/src/wpilibsharp/CAN.cs b/src/wpilibsharp/CAN.cs deleted file mode 100644 index 3398aecf..00000000 --- a/src/wpilibsharp/CAN.cs +++ /dev/null @@ -1,31 +0,0 @@ -using System; -using Hal; - -namespace WPILib -{ - public sealed class CAN : IDisposable - { - public const CANManufacturer TeamManufacturer = CANManufacturer.kTeamUse; - public const CANDeviceType TeamDeviceType = CANDeviceType.kMiscellaneous; - - private readonly int m_handle; - - public CAN(int deviceId, CANManufacturer manufacturer = TeamManufacturer, CANDeviceType deviceType = TeamDeviceType) - { - m_handle = Hal.CANAPILowLevel.Initialize(manufacturer, deviceId, deviceType); - UsageReporting.Report(ResourceType.CAN, deviceId + 1); - } - - - - public void Dispose() - { - Hal.CANAPILowLevel.Clean(m_handle); - } - - public void WritePacket(ReadOnlySpan data, int apiId) - { - Hal.CANAPILowLevel.WritePacket(m_handle, data, apiId); - } - } -} diff --git a/src/wpilibsharp/Counters/ExternalDirectionCounter.cs b/src/wpilibsharp/Counters/ExternalDirectionCounter.cs deleted file mode 100644 index 838c2390..00000000 --- a/src/wpilibsharp/Counters/ExternalDirectionCounter.cs +++ /dev/null @@ -1,78 +0,0 @@ -using System; -using WPILib.SmartDashboardNS; - -namespace WPILib.Counters -{ - public sealed class ExternalDirectionCounter : ISendable, IDisposable - { - private IDigitalSource m_countSource; - private IDigitalSource m_directionSource; - private readonly int m_handle; - private readonly int m_index; - - public ExternalDirectionCounter(IDigitalSource countSource, IDigitalSource directionSource) - { - m_countSource = countSource ?? throw new ArgumentNullException(nameof(countSource)); - m_directionSource = directionSource ?? throw new ArgumentNullException(nameof(directionSource)); - m_handle = Hal.CounterLowLevel.Initialize(Hal.CounterMode.kExternalDirection, out m_index); - - Hal.CounterLowLevel.SetUpSource(m_handle, m_countSource.PortHandleForRouting, m_countSource.AnalogTriggerTypeForRouting); - Hal.CounterLowLevel.SetUpSourceEdge(m_handle, true, false); - Hal.CounterLowLevel.SetDownSource(m_handle, m_directionSource.PortHandleForRouting, m_directionSource.AnalogTriggerTypeForRouting); - Hal.CounterLowLevel.SetDownSourceEdge(m_handle, false, true); - Hal.CounterLowLevel.Reset(m_handle); - } - - public IDigitalSource CountSource - { - set - { - m_countSource = value ?? throw new ArgumentNullException(nameof(value)); - Hal.CounterLowLevel.SetUpSource(m_handle, m_countSource.PortHandleForRouting, m_countSource.AnalogTriggerTypeForRouting); - Hal.CounterLowLevel.SetUpSourceEdge(m_handle, true, false); - } - } - - public EdgeConfiguration EdgeConfiguration - { - set - { - Hal.CounterLowLevel.SetUpSourceEdge(m_handle, (value & EdgeConfiguration.kRisingEdge) != 0, (value & EdgeConfiguration.kFallingEdge) != 0); - } - } - - public IDigitalSource DirectionSource - { - set - { - m_directionSource = value ?? throw new ArgumentNullException(nameof(value)); - Hal.CounterLowLevel.SetDownSource(m_handle, m_directionSource.PortHandleForRouting, m_directionSource.AnalogTriggerTypeForRouting); - } - } - - public void Reset() - { - Hal.CounterLowLevel.Reset(m_handle); - } - - public bool ReverseDirection - { - set - { - Hal.CounterLowLevel.SetReverseDirection(m_handle, value); - } - } - - public int Count => Hal.CounterLowLevel.Get(m_handle); - - public void Dispose() - { - Hal.CounterLowLevel.Free(m_handle); - } - - void ISendable.InitSendable(ISendableBuilder builder) - { - - } - } -} diff --git a/src/wpilibsharp/Counters/Tachometer.cs b/src/wpilibsharp/Counters/Tachometer.cs deleted file mode 100644 index 4bc36a68..00000000 --- a/src/wpilibsharp/Counters/Tachometer.cs +++ /dev/null @@ -1,69 +0,0 @@ -using System; -using UnitsNet; -using WPILib.SmartDashboardNS; - -namespace WPILib.Counters -{ - public sealed class Tachometer : IDisposable, ISendable - { - private readonly IDigitalSource m_source; - private readonly int m_handle; - private readonly int m_index; - - public Tachometer(IDigitalSource source) - { - m_source = source ?? throw new ArgumentNullException(nameof(source)); - m_handle = Hal.CounterLowLevel.Initialize(Hal.CounterMode.kTwoPulse, out m_index); - - Hal.CounterLowLevel.SetUpSource(m_handle, m_source.PortHandleForRouting, m_source.AnalogTriggerTypeForRouting); - Hal.CounterLowLevel.SetUpSourceEdge(m_handle, true, false); - } - - public TimeSpan Period => TimeSpan.FromSeconds(Hal.CounterLowLevel.GetPeriod(m_handle)); - - public Frequency Frequency => Frequency.FromHertz(1 / Hal.CounterLowLevel.GetPeriod(m_handle)); - - public int EdgesPerRevolution { get; set; } = 1; - - public RotationalSpeed? RotationalSpeed - { - get - { - var period = Hal.CounterLowLevel.GetPeriod(m_handle); - if (period == 0) - { - return null; - } - return UnitsNet.RotationalSpeed.FromRevolutionsPerSecond((1.0 / EdgesPerRevolution) / period); - - } - } - - public bool Stopped => Hal.CounterLowLevel.GetStopped(m_handle); - - public int SamplesToAverage - { - get => Hal.CounterLowLevel.GetSamplesToAverage(m_handle); - set => Hal.CounterLowLevel.SetSamplesToAverage(m_handle, value); - } - - public TimeSpan MaxPeriod - { - set => Hal.CounterLowLevel.SetMaxPeriod(m_handle, value.TotalSeconds); - } - - public bool UpdateWhenEmpty - { - set => Hal.CounterLowLevel.SetUpdateWhenEmpty(m_handle, value); - } - - public void Dispose() - { - Hal.CounterLowLevel.Free(m_handle); - } - - void ISendable.InitSendable(ISendableBuilder builder) - { - } - } -} diff --git a/src/wpilibsharp/Counters/UpDownCounter.cs b/src/wpilibsharp/Counters/UpDownCounter.cs deleted file mode 100644 index 17c29d7c..00000000 --- a/src/wpilibsharp/Counters/UpDownCounter.cs +++ /dev/null @@ -1,97 +0,0 @@ -using System; -using WPILib.SmartDashboardNS; - -namespace WPILib.Counters -{ - public sealed class UpDownCounter : ISendable, IDisposable - { - - private IDigitalSource? m_upSource; - private IDigitalSource? m_downSource; - private readonly int m_handle; - private readonly int m_index; - - public UpDownCounter() - { - m_handle = Hal.CounterLowLevel.Initialize(Hal.CounterMode.kTwoPulse, out m_index); - Hal.CounterLowLevel.Reset(m_handle); - } - - public IDigitalSource? UpSource - { - set - { - if (value == null) - { - Hal.CounterLowLevel.ClearUpSource(m_handle); - m_upSource = null; - } - else - { - m_upSource = value; - Hal.CounterLowLevel.SetUpSource(m_handle, m_upSource.PortHandleForRouting, m_upSource.AnalogTriggerTypeForRouting); - Hal.CounterLowLevel.SetUpSourceEdge(m_handle, true, false); - } - } - } - - public IDigitalSource? DownSource - { - set - { - if (value == null) - { - Hal.CounterLowLevel.ClearDownSource(m_handle); - m_downSource = null; - } - else - { - m_downSource = value; - Hal.CounterLowLevel.SetDownSource(m_handle, m_downSource.PortHandleForRouting, m_downSource.AnalogTriggerTypeForRouting); - Hal.CounterLowLevel.SetDownSourceEdge(m_handle, true, false); - } - } - } - - public EdgeConfiguration UpEdgeConfiguration - { - set - { - Hal.CounterLowLevel.SetUpSourceEdge(m_handle, (value & EdgeConfiguration.kRisingEdge) != 0, (value & EdgeConfiguration.kFallingEdge) != 0); - } - } - - public EdgeConfiguration DownEdgeConfiguration - { - set - { - Hal.CounterLowLevel.SetDownSourceEdge(m_handle, (value & EdgeConfiguration.kRisingEdge) != 0, (value & EdgeConfiguration.kFallingEdge) != 0); - } - } - - - public void Reset() - { - Hal.CounterLowLevel.Reset(m_handle); - } - - public bool ReverseDirection - { - set - { - Hal.CounterLowLevel.SetReverseDirection(m_handle, value); - } - } - - public int Count => Hal.CounterLowLevel.Get(m_handle); - - public void Dispose() - { - Hal.CounterLowLevel.Free(m_handle); - } - - void ISendable.InitSendable(ISendableBuilder builder) - { - } - } -} diff --git a/src/wpilibsharp/DigitalGlitchFilter.cs b/src/wpilibsharp/DigitalGlitchFilter.cs deleted file mode 100644 index 0bc75788..00000000 --- a/src/wpilibsharp/DigitalGlitchFilter.cs +++ /dev/null @@ -1,107 +0,0 @@ -using System; -using Hal; -using WPILib.SmartDashboardNS; - -namespace WPILib -{ - public sealed class DigitalGlitchFilter : ISendable, IDisposable - { - private static readonly object m_mutex = new object(); - private static readonly bool[] m_filterAllocated = new bool[3]; - - private int m_channelIndex = -1; - - public DigitalGlitchFilter() - { - lock (m_mutex) - { - int index = 0; - while (m_filterAllocated[index] && index < m_filterAllocated.Length) - { - index++; - } - if (index != m_filterAllocated.Length) - { - m_channelIndex = index; - m_filterAllocated[index] = true; - UsageReporting.Report(ResourceType.DigitalGlitchFilter, m_channelIndex + 1); - SendableRegistry.Instance.AddLW(this, "DigitalGlitchFilter", index); - } - else - { - throw new InvalidOperationException("Out of filters"); - } - } - } - - private static void SetFilter(IDigitalSource input, int channelIndex) - { - if (input != null) - { - if (input.IsAnalogTrigger) - { - throw new InvalidOperationException("Analog Triggers not supported for DigitalGlitchFilters"); - } - Hal.DIOLowLevel.SetFilterSelect(input.PortHandleForRouting, channelIndex); - - int selected = DIOLowLevel.GetFilterSelect(input.PortHandleForRouting); - if (selected != channelIndex) - { - throw new InvalidOperationException($"SetFilterSelect {channelIndex} failed -> {selected}"); - } - } - } - - public void Add(IDigitalSource input) - { - SetFilter(input, m_channelIndex + 1); - } - -#pragma warning disable CA1822 // Mark members as static - public void Remove(IDigitalSource input) -#pragma warning restore CA1822 // Mark members as static - { - SetFilter(input, 0); - } - - public void SetPeriodCycles(long fpgaCycles) - { - Console.WriteLine(fpgaCycles); - DIOLowLevel.SetFilterPeriod(m_channelIndex, fpgaCycles); - } - - public void SetPeriod(TimeSpan timeSpan) - { - // Ticks are in 100ns periods. - // 4 is because an fpga cycle is 25ns - long fpgaCycles = timeSpan.Ticks * 4; - SetPeriodCycles(fpgaCycles); - } - - public void SetPeriodNanoseconds(long nanoseconds) - { - // x 25 to get to 40MHz - long fpgaCycles = nanoseconds * 25; - SetPeriodCycles(fpgaCycles); - } - - - public void Dispose() - { - SendableRegistry.Instance.Remove(this); - if (m_channelIndex > 0) - { - lock (m_mutex) - { - m_filterAllocated[m_channelIndex] = false; - } - m_channelIndex = -1; - } - - } - - void ISendable.InitSendable(ISendableBuilder builder) - { - } - } -} diff --git a/src/wpilibsharp/DigitalInput.cs b/src/wpilibsharp/DigitalInput.cs deleted file mode 100644 index 0d874f71..00000000 --- a/src/wpilibsharp/DigitalInput.cs +++ /dev/null @@ -1,39 +0,0 @@ -using System; -using Hal; -using WPILib.SmartDashboardNS; - -namespace WPILib -{ - public sealed class DigitalInput : IDigitalSource, IDisposable, ISendable - { - private readonly int m_handle; - - public DigitalInput(int channel) - { - m_handle = Hal.DIOLowLevel.InitializePort(Hal.HALLowLevel.GetPort(channel), true); - - UsageReporting.Report(ResourceType.DigitalInput, channel + 1); - SendableRegistry.Instance.AddLW(this, "DigitalInput", channel); - } - - public bool IsAnalogTrigger => false; - - public int Channel { get; } - - public AnalogTriggerType AnalogTriggerTypeForRouting => 0; - - public int PortHandleForRouting => m_handle; - - public bool Output => Hal.DIOLowLevel.Get(m_handle); - - public void Dispose() - { - SendableRegistry.Instance.Remove(this); - Hal.DIOLowLevel.FreePort(m_handle); - } - - void ISendable.InitSendable(ISendableBuilder builder) - { - } - } -} diff --git a/src/wpilibsharp/DriverStation.cs b/src/wpilibsharp/DriverStation.cs deleted file mode 100644 index 4ef1f6a5..00000000 --- a/src/wpilibsharp/DriverStation.cs +++ /dev/null @@ -1,671 +0,0 @@ -using Hal; -using NetworkTables; -using System; -using System.Threading; -using UnitsNet; -using WPIUtil; - -namespace WPILib -{ - public enum Alliance { kRed, kBlue, kInvalid } - public enum MatchType { kNone, kPractice, kQualification, kElimination } - -#pragma warning disable CA1822 // Method could be marked static - public sealed class DriverStation : IDisposable - { - - - private static readonly Lazy lazyInstance = new Lazy(() => - { - return new DriverStation(); - }, LazyThreadSafetyMode.ExecutionAndPublication); - - public static DriverStation Instance => lazyInstance.Value; - - public static void ReportError(string error, bool printTrace) - { - ReportErrorImpl(true, 1, error, printTrace); - } - - public static void ReportError(string error, string stackTrace) - { - ReportErrorImpl(true, 1, error, stackTrace); - } - - public static void ReportWarning(string error, bool printTrace) - { - ReportErrorImpl(false, 1, error, printTrace); - } - - public static void ReportWarning(string error, string stackTrace) - { - ReportErrorImpl(false, 1, error, stackTrace); - } - - private static void ReportErrorImpl(bool isError, int code, string error, bool printTrace) - { - ReportErrorImpl(isError, code, error, printTrace, System.Environment.StackTrace, 3); - } - - private static void ReportErrorImpl(bool isError, int code, string error, string stackTrace) - { - ReportErrorImpl(isError, code, error, true, stackTrace, 0); - } - - private static void ReportErrorImpl(bool isError, int code, string error, bool printTrace, string stackTrace, int stackTraceFirst) - { - var _ = stackTraceFirst; - ReadOnlySpan printedTrace = stackTrace.AsSpan(); - if (!printTrace) - { - printedTrace = "".AsSpan(); - } - Hal.DriverStationLowLevel.SendError(isError, code, false, error.AsSpan(), "".AsSpan(), printedTrace, true); - } - - private readonly struct MatchDataSender - { - public readonly NetworkTable table; - public readonly NetworkTableEntry typeMetadata; - public readonly NetworkTableEntry gameSpecificMessage; - public readonly NetworkTableEntry eventName; - public readonly NetworkTableEntry matchNumber; - public readonly NetworkTableEntry replayNumber; - public readonly NetworkTableEntry matchType; - public readonly NetworkTableEntry alliance; - public readonly NetworkTableEntry station; - public readonly NetworkTableEntry controlWord; - - public MatchDataSender(NetworkTableInstance inst) - { - table = inst.GetTable("FMSInfo"); - typeMetadata = table.GetEntry(".type"); - typeMetadata.ForceSetString("FMSInfo"); - gameSpecificMessage = table.GetEntry("GameSpecificMessage"); - gameSpecificMessage.ForceSetString(""); - eventName = table.GetEntry("EventName"); - eventName.ForceSetString(""); - matchNumber = table.GetEntry("MatchNumber"); - matchNumber.ForceSetDouble(0); - replayNumber = table.GetEntry("ReplayNumber"); - replayNumber.ForceSetDouble(0); - matchType = table.GetEntry("MatchType"); - matchType.ForceSetDouble(0); - alliance = table.GetEntry("IsRedAlliance"); - alliance.ForceSetBoolean(true); - station = table.GetEntry("StationNumber"); - station.ForceSetDouble(1); - controlWord = table.GetEntry("FMSControlData"); - controlWord.ForceSetDouble(0); - } - } - - private static readonly TimeSpan JoystickUnpluggedMessageInterval = TimeSpan.FromSeconds(1); - public static readonly int NumJoystickPorts = 6; - public static readonly int MaxJoystickAxes = 12; - public static readonly int MaxJoystickPOVs = 12; - - - private bool m_userInDisabled = false; - private bool m_userInAutonomous = false; - private bool m_userInTeleop = false; - private bool m_userInTest = false; - private readonly MatchDataSender m_matchDataSender; - - private DriverStation() - { - m_waitForDataCounter = 0; - m_matchDataSender = new MatchDataSender(NetworkTableInstance.Default); - - m_dsThread = new Thread(Run) - { - Name = "Driver Station", - IsBackground = true - }; - m_dsThread.Start(); - } - - private void ValidateJoystickPort(int stick) - { - if (stick < 0 || stick > NumJoystickPorts) - { - throw new ArgumentOutOfRangeException(nameof(stick), $"Joystick index is out of range, should be 0-5"); - } - } - - private bool ValidateButtonIndexValid(int button) - { - if (button <= 0) - { - ReportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C#"); - return false; - } - return true; - } - - private bool ValidateAxisIndexValid(int axis) - { - if (axis < 0 || axis > MaxJoystickAxes) - { - ReportJoystickUnpluggedError("Joystick axis is out of range"); - return false; - } - return true; - } - - private bool ValidatePOVIndexValid(int axis) - { - if (axis < 0 || axis > MaxJoystickPOVs) - { - ReportJoystickUnpluggedError("Joystick pov is out of range"); - return false; - } - return true; - } - - private bool ValidateButtonIsFound(int button, int count, int stick) - { - if (button >= count) - { - ReportJoystickUnpluggedWarning("Joystick Button " + button + " on port " + stick - + " not available, check if controller is plugged in"); - return false; - } - return true; - } - - private bool ValidateAxisIsFound(int axis, int count, int stick) - { - if (axis > count) - { - ReportJoystickUnpluggedWarning("Joystick axis " + axis + " on port " + stick - + " not available, check if controller is plugged in"); - return false; - } - return true; - } - - private bool ValidatePOVIsFound(int pov, int count, int stick) - { - if (pov > count) - { - ReportJoystickUnpluggedWarning("Joystick axis " + pov + " on port " + stick - + " not available, check if controller is plugged in"); - return false; - } - return true; - } - - public bool GetStickButton(int stick, int button) - { - ValidateJoystickPort(stick); - if (!ValidateButtonIndexValid(button)) - { - return false; - } - - var buttons = Hal.DriverStationLowLevel.GetJoystickButtons(stick); - - if (!ValidateButtonIsFound(button, buttons.Count, stick)) - { - return false; - } - - return buttons.GetButton(button); - } - - public bool GetStickButtonPressed(int stick, int button) - { - ValidateJoystickPort(stick); - if (!ValidateButtonIndexValid(button)) - { - return false; - } - - var buttons = Hal.DriverStationLowLevel.GetJoystickButtons(stick); - - if (!ValidateButtonIsFound(button, buttons.Count, stick)) - { - return false; - } - - lock (m_buttonEdgeMutex) - { - if ((m_joystickButtonsPressed[stick] & 1u << (button - 1)) != 0) - { - m_joystickButtonsPressed[stick] &= ~(1u << (button - 1)); - return true; - } - return false; - } - } - - public bool GetStickButtonReleased(int stick, int button) - { - ValidateJoystickPort(stick); - if (!ValidateButtonIndexValid(button)) - { - return false; - } - - var buttons = Hal.DriverStationLowLevel.GetJoystickButtons(stick); - - if (!ValidateButtonIsFound(button, buttons.Count, stick)) - { - return false; - } - - lock (m_buttonEdgeMutex) - { - if ((m_joystickButtonsReleased[stick] & 1u << (button - 1)) != 0) - { - m_joystickButtonsReleased[stick] &= ~(1u << (button - 1)); - return true; - } - return false; - } - } - - public double GetStickAxis(int stick, int axis) - { - ValidateJoystickPort(stick); - if (!ValidateAxisIndexValid(axis)) - { - return 0.0; - } - - var axes = Hal.DriverStationLowLevel.GetJoystickAxes(stick); - - if (!ValidateAxisIsFound(axis, axes.Count, stick)) - { - return 0.0; - } - - unsafe - { - return axes.Axes[axis]; - } - } - - public int GetStickPOV(int stick, int pov) - { - ValidateJoystickPort(stick); - if (!ValidatePOVIndexValid(pov)) - { - return -1; - } - - var povs = Hal.DriverStationLowLevel.GetJoystickPOVs(stick); - - if (!ValidateAxisIsFound(pov, povs.Count, stick)) - { - return -1; - } - - unsafe - { - return povs.POVs[pov]; - } - } - - public uint GetStickButtons(int stick) - { - ValidateJoystickPort(stick); - - return Hal.DriverStationLowLevel.GetJoystickButtons(stick).Buttons; - } - - public int GetStickAxisCount(int stick) - { - ValidateJoystickPort(stick); - - return Hal.DriverStationLowLevel.GetJoystickAxes(stick).Count; - } - - public int GetStickPOVCount(int stick) - { - ValidateJoystickPort(stick); - - return Hal.DriverStationLowLevel.GetJoystickPOVs(stick).Count; - } - - public int GetStickButtonCount(int stick) - { - ValidateJoystickPort(stick); - - return Hal.DriverStationLowLevel.GetJoystickButtons(stick).Count; - } - - public bool GetJoystickIsXbox(int stick) - { - ValidateJoystickPort(stick); - - return Hal.DriverStationLowLevel.GetJoystickIsXbox(stick); - } - - public int GetJoystickType(int stick) - { - ValidateJoystickPort(stick); - - return Hal.DriverStationLowLevel.GetJoystickType(stick); - } - - public string GetJoystickName(int stick) - { - ValidateJoystickPort(stick); - - return Hal.DriverStationLowLevel.GetJoystickName(stick); - } - - public int GetJoystickAxisType(int stick, int axis) - { - ValidateJoystickPort(stick); - - if (!ValidateAxisIndexValid(axis)) - { - return 0; - } - - return Hal.DriverStationLowLevel.GetJoystickAxisType(stick, axis); - } - - public bool IsEnabled => Hal.DriverStationLowLevel.GetControlWord().Enabled; - public bool IsDisabled => !IsEnabled; - public bool IsEStopped => Hal.DriverStationLowLevel.GetControlWord().EStop; - public bool IsAutonomous => Hal.DriverStationLowLevel.GetControlWord().Autonomous; - public bool IsOperatorControl => Hal.DriverStationLowLevel.GetControlWord().FmsAttached; - public bool IsTest => Hal.DriverStationLowLevel.GetControlWord().Test; - public bool IsDSAttached => Hal.DriverStationLowLevel.GetControlWord().DsAttached; - public bool IsNewControlData => Hal.DriverStationLowLevel.IsNewControlData(); - public bool IsFMSAttached => Hal.DriverStationLowLevel.GetControlWord().FmsAttached; - - public string GameSpecificMessage - { - get - { - unsafe - { - var matchInfo = Hal.DriverStationLowLevel.GetMatchInfo(); - return UTF8String.ReadUTF8String(matchInfo.GameSpecificMessage, matchInfo.GameSpecificMessageSize); - } - } - } - - public string EventName - { - get - { - unsafe - { - var matchInfo = Hal.DriverStationLowLevel.GetMatchInfo(); - return UTF8String.ReadUTF8String(matchInfo.EventName); - } - } - } - - public MatchType MatchType => (MatchType)Hal.DriverStationLowLevel.GetMatchInfo().MatchType; - - public int MatchNumber => Hal.DriverStationLowLevel.GetMatchInfo().MatchNumber; - public int ReplayNumber => Hal.DriverStationLowLevel.GetMatchInfo().ReplayNumber; - public Alliance Alliance - { - get - { - var allianceId = Hal.DriverStationLowLevel.GetAllianceStation(); - switch (allianceId) - { - case AllianceStationID.kRed1: - case AllianceStationID.kRed2: - case AllianceStationID.kRed3: - return Alliance.kRed; - case AllianceStationID.kBlue1: - case AllianceStationID.kBlue2: - case AllianceStationID.kBlue3: - return Alliance.kBlue; - default: - return Alliance.kInvalid; - } - } - } - public int Location - { - get - { - var allianceId = Hal.DriverStationLowLevel.GetAllianceStation(); - switch (allianceId) - { - case AllianceStationID.kRed1: - case AllianceStationID.kBlue1: - return 1; - case AllianceStationID.kRed2: - case AllianceStationID.kBlue2: - return 2; - case AllianceStationID.kRed3: - case AllianceStationID.kBlue3: - return 3; - default: - return 0; - } - } - } - - public void WaitForData() - { - WaitForData(TimeSpan.Zero); - } - - - public bool WaitForData(TimeSpan timeout) - { - var timeoutTime = Timer.FPGATimestamp + timeout; - - lock (m_waitForDataMutex) - { - int currentCount = m_waitForDataCounter; - while (m_waitForDataCounter == currentCount) - { - if (timeout != TimeSpan.Zero) - { - var currentTime = Timer.FPGATimestamp; - if (currentTime >= timeoutTime) - { - return false; - } - - var timeToWait = timeoutTime - currentTime; - if (!Monitor.Wait(m_waitForDataMutex, timeToWait)) - { - return false; - } - } - else - { - Monitor.Wait(m_waitForDataMutex); - } - } - return true; - } - } - - public TimeSpan MatchTime => TimeSpan.FromSeconds(Hal.DriverStationLowLevel.GetMatchTime()); - public ElectricPotential BatteryVoltage => ElectricPotential.FromVolts(Hal.PowerLowLevel.GetVinVoltage()); - - public void InDisabled(bool entering) - { - m_userInDisabled = entering; - } - - public void InAutonomous(bool entering) - { - m_userInAutonomous = entering; - } - - public void InOperatorControl(bool entering) - { - m_userInTeleop = entering; - } - - public void InTest(bool entering) - { - m_userInTest = entering; - } - - public void WakeupWaitForData() - { - lock (m_waitForDataMutex) - { - m_waitForDataCounter++; - Monitor.PulseAll(m_waitForDataMutex); - } - } - - private void GetData() - { - lock (m_buttonEdgeMutex) - { - for (int i = 0; i < NumJoystickPorts; i++) - { - var currentButtons = Hal.DriverStationLowLevel.GetJoystickButtons(i); - - m_joystickButtonsPressed[i] |= ~m_previousButtonStates[i].Buttons & currentButtons.Buttons; - - m_joystickButtonsReleased[i] |= m_previousButtonStates[i].Buttons & ~currentButtons.Buttons; - - m_previousButtonStates[i] = currentButtons; - } - } - - WakeupWaitForData(); - SendMatchData(); - } - - - private void ReportJoystickUnpluggedError(string message) - { - var currentTime = Timer.FPGATimestamp; - if (currentTime > m_nextMessageTime) - { - ReportError(message, false); - m_nextMessageTime = currentTime + JoystickUnpluggedMessageInterval; - } - } - - private void ReportJoystickUnpluggedWarning(string message) - { - var currentTime = Timer.FPGATimestamp; - if (currentTime > m_nextMessageTime) - { - ReportWarning(message, false); - m_nextMessageTime = currentTime + JoystickUnpluggedMessageInterval; - } - } - - private void Run() - { - m_isRunning = true; - int safetyCounter = 0; - while (m_isRunning) - { - Hal.DriverStationLowLevel.WaitForDSData(); - GetData(); - - if (IsDisabled) - { - safetyCounter = 0; - } - - if (++safetyCounter >= 4) - { - MotorSafety.CheckMotors(); - safetyCounter = 0; - } - - if (m_userInDisabled) Hal.DriverStationLowLevel.ObserveUserProgramDisabled(); - if (m_userInAutonomous) Hal.DriverStationLowLevel.ObserveUserProgramAutonomous(); - if (m_userInTeleop) Hal.DriverStationLowLevel.ObserveUserProgramTeleop(); - if (m_userInTest) Hal.DriverStationLowLevel.ObserveUserProgramTest(); - } - } - - private unsafe void SendMatchData() - { - var alliance = Hal.DriverStationLowLevel.GetAllianceStation(); - bool isRedAlliance; - int stationNumber; - switch (alliance) - { - case AllianceStationID.kBlue1: - isRedAlliance = false; - stationNumber = 1; - break; - case AllianceStationID.kBlue2: - isRedAlliance = false; - stationNumber = 2; - break; - case AllianceStationID.kBlue3: - isRedAlliance = false; - stationNumber = 3; - break; - case AllianceStationID.kRed1: - isRedAlliance = true; - stationNumber = 1; - break; - case AllianceStationID.kRed2: - isRedAlliance = true; - stationNumber = 2; - break; - default: - isRedAlliance = true; - stationNumber = 3; - break; - } - - var tmpDataStore = Hal.DriverStationLowLevel.GetMatchInfo(); - - m_matchDataSender.alliance.SetBoolean(isRedAlliance); - m_matchDataSender.station.SetDouble(stationNumber); - int count = 0; - var lenToCheck = tmpDataStore.EventName; - while (count < 64) - { - if (lenToCheck[count] == 0) - { - break; - } - count++; - } - m_matchDataSender.eventName.SetStringDirect(tmpDataStore.EventName, count); - m_matchDataSender.gameSpecificMessage.SetStringDirect(tmpDataStore.GameSpecificMessage, tmpDataStore.GameSpecificMessageSize); - - m_matchDataSender.matchNumber.SetDouble(tmpDataStore.MatchNumber); - m_matchDataSender.replayNumber.SetDouble(tmpDataStore.ReplayNumber); - m_matchDataSender.matchType.SetDouble((int)tmpDataStore.MatchType); - - var ctlWord = Hal.DriverStationLowLevel.GetControlWord(); - m_matchDataSender.controlWord.SetDouble(ctlWord.Word); - } - - public void Dispose() - { - m_isRunning = false; - Hal.DriverStationLowLevel.ReleaseDSMutex(); - m_dsThread.Join(); - } - - private readonly object m_buttonEdgeMutex = new object(); - private readonly JoystickButtons[] m_previousButtonStates = new JoystickButtons[6]; - private readonly uint[] m_joystickButtonsPressed = new uint[6]; - private readonly uint[] m_joystickButtonsReleased = new uint[6]; - - private readonly Thread m_dsThread; - private bool m_isRunning = false; - - private readonly object m_waitForDataMutex = new object(); - private int m_waitForDataCounter = 0; - private TimeSpan m_nextMessageTime = TimeSpan.Zero; - - - - } -#pragma warning restore CA1822 // Method could be marked static -} diff --git a/src/wpilibsharp/DutyCycle.cs b/src/wpilibsharp/DutyCycle.cs deleted file mode 100644 index d571be72..00000000 --- a/src/wpilibsharp/DutyCycle.cs +++ /dev/null @@ -1,53 +0,0 @@ -using System; -using Hal; -using UnitsNet; -using WPILib.SmartDashboardNS; - -namespace WPILib -{ - public sealed class DutyCycle : ISendable, IDisposable - { - internal readonly int m_handle; - - private readonly IDigitalSource m_source; - - public DutyCycle(IDigitalSource digitalSource) - { - if (digitalSource == null) - { - throw new ArgumentNullException(nameof(digitalSource)); - } - - m_handle = Hal.DutyCycleLowLevel.Initialize(digitalSource.PortHandleForRouting, digitalSource.AnalogTriggerTypeForRouting); - - m_source = digitalSource; - var index = FPGAIndex; - UsageReporting.Report(ResourceType.DutyCycle, index + 1); - SendableRegistry.Instance.AddLW(this, "Duty Cycle", index); - } - - public void Dispose() - { - Hal.DutyCycleLowLevel.Free(m_handle); - } - - public Frequency Frequency => Frequency.FromHertz(Hal.DutyCycleLowLevel.GetFrequency(m_handle)); - - public double Output => Hal.DutyCycleLowLevel.GetOutput(m_handle); - - public int OutputRaw => Hal.DutyCycleLowLevel.GetOutputRaw(m_handle); - - public int OutputScaleFactor => Hal.DutyCycleLowLevel.GetOutputScaleFactor(m_handle); - - public int FPGAIndex => Hal.DutyCycleLowLevel.GetFPGAIndex(m_handle); - - public int SourceChannel => m_source.Channel; - - void ISendable.InitSendable(ISendableBuilder builder) - { - builder.SmartDashboardType = "Duty Cycle"; - builder.AddDoubleProperty("Frequency", () => Frequency.Hertz, null); - builder.AddDoubleProperty("Output", () => Output, null); - } - } -} diff --git a/src/wpilibsharp/DutyCycleEncoder.cs b/src/wpilibsharp/DutyCycleEncoder.cs deleted file mode 100644 index e90cb6ad..00000000 --- a/src/wpilibsharp/DutyCycleEncoder.cs +++ /dev/null @@ -1,148 +0,0 @@ -using System; -using Hal; -using UnitsNet; -using WPILib.Counters; -using WPILib.SmartDashboardNS; - -namespace WPILib -{ - public sealed class DutyCycleEncoder : ISendable, IDisposable - { - private readonly DutyCycle m_dutyCycle; - private readonly bool m_ownsDutyCycle; - private readonly AnalogTrigger m_analogTrigger; - private readonly UpDownCounter m_counter; - - private Frequency m_frequencyThreshold = Frequency.FromHertz(100); - private double m_positionOffset; - private readonly double m_distancePerRotation = 1.0; - private Angle m_lastPosition; - - private readonly SimDevice m_simDevice; - - public DutyCycleEncoder(int channel) -#pragma warning disable CA2000 // Dispose objects before losing scope - : this(new DigitalInput(channel)) -#pragma warning restore CA2000 // Dispose objects before losing scope - { - - } - - public DutyCycleEncoder(IDigitalSource source) - : this(new DutyCycle(source)) - { - m_ownsDutyCycle = true; - } - - public DutyCycleEncoder(DutyCycle dutyCycle) - { - m_dutyCycle = dutyCycle; - m_analogTrigger = new AnalogTrigger(m_dutyCycle); - m_counter = new UpDownCounter(); - - m_simDevice = SimDevice.Create("DutyCycleEncoder", m_dutyCycle.FPGAIndex); - - if (m_simDevice) - { - - } - - m_analogTrigger.SetLimitsDutyCycle(0.25, 0.75); - m_counter.UpSource = m_analogTrigger.CreateOutput(AnalogTriggerType.kRisingPulse); - m_counter.DownSource = m_analogTrigger.CreateOutput(AnalogTriggerType.kFallingPulse); - - SendableRegistry.Instance.AddLW(this, "DutyCycle Encoder", m_dutyCycle.SourceChannel); - } - - public void Reset() - { - m_counter.Reset(); - m_positionOffset = m_dutyCycle.Output; - } - - public Angle Output - { - get - { - for (int i = 0; i < 10; i++) - { - double counter = m_counter.Count; - double pos = m_dutyCycle.Output; - double counter2 = m_counter.Count; - double pos2 = m_dutyCycle.Output; - if (counter == counter2 && pos == pos2) - { - double position = counter + pos - m_positionOffset; - var anglePos = Angle.FromRevolutions(position); - m_lastPosition = anglePos; - return anglePos; - } - } - return m_lastPosition; - } - } - - public double PositionOffset => m_positionOffset; - - public double Distance - { - get - { - return Output.Revolutions * DistancePerRotation; - } - } - - public double DistancePerRotation - { - get - { - return m_distancePerRotation; - } - } - - public Frequency Frequency => m_dutyCycle.Frequency; - - public Frequency ConnectedFrequencyThreshold - { - get => m_frequencyThreshold; - set - { - if (value < Frequency.Zero) - { - value = Frequency.Zero; - } - m_frequencyThreshold = value; - } - } - - public bool IsConnected - { - get - { - return Frequency > m_frequencyThreshold; - } - } - - public void Dispose() - { - m_counter.Dispose(); - m_analogTrigger.Dispose(); - if (m_ownsDutyCycle) - { - m_dutyCycle.Dispose(); - } - if (m_simDevice) - { - m_simDevice.Dispose(); - } - } - - void ISendable.InitSendable(ISendableBuilder builder) - { - builder.SmartDashboardType = "AbsoluteEncoder"; - builder.AddDoubleProperty("Distance", () => Distance, null); - builder.AddDoubleProperty("Distance Per Rotation", () => DistancePerRotation, null); - builder.AddBooleanProperty("Is Connected", () => IsConnected, null); - } - } -} diff --git a/src/wpilibsharp/EdgeConfiguration.cs b/src/wpilibsharp/EdgeConfiguration.cs deleted file mode 100644 index 9929ad9c..00000000 --- a/src/wpilibsharp/EdgeConfiguration.cs +++ /dev/null @@ -1,15 +0,0 @@ -using System; - -namespace WPILib -{ - [Flags] -#pragma warning disable CA1714 // Flags enums should have plural names - public enum EdgeConfiguration -#pragma warning restore CA1714 // Flags enums should have plural names - { - kNone = 0x0, - kRisingEdge = 0x1, - kFallingEdge = 0x10, - kBothEdges = kRisingEdge | kFallingEdge - } -} diff --git a/src/wpilibsharp/GenericHID.cs b/src/wpilibsharp/GenericHID.cs deleted file mode 100644 index 90dbd543..00000000 --- a/src/wpilibsharp/GenericHID.cs +++ /dev/null @@ -1,130 +0,0 @@ -namespace WPILib -{ - public abstract class GenericHID - { - public enum RumbleType - { - kLeftRumble, - kRightRumble - } - - public enum HIDType - { - kUnknown = -1, - kXInputUnknown = 0, - kXInputGamepad = 1, - kXInputWheel = 2, - kXInputArcadeStick = 3, - kXInputFlightStick = 4, - kXInputDancePad = 5, - kXInputGuitar = 6, - kXInputGuitar2 = 7, - kXInputDrumKit = 8, - kXInputGuitar3 = 11, - kXInputArcadePad = 19, - kHIDJoystick = 20, - kHIDGamepad = 21, - kHIDDriving = 22, - kHIDFlight = 23, - kHID1stPerson = 24 - }; - - public enum JoystickHand { kLeftHand = 0, kRightHand = 1 }; - - - private readonly DriverStation m_ds; - private int m_outputs; - private short m_leftRumble; - private short m_rightRumble; - - public GenericHID(int port) - { - m_ds = DriverStation.Instance; - Port = port; - } - - public double GetX() - { - return GetX(JoystickHand.kRightHand); - } - - public abstract double GetX(JoystickHand hand); - - public double GetY() - { - return GetY(JoystickHand.kRightHand); - } - - public abstract double GetY(JoystickHand hand); - - public bool GetRawButton(int button) - { - return m_ds.GetStickButton(Port, button); - } - - public bool GetRawButtonPressed(int button) - { - return m_ds.GetStickButtonPressed(Port, button); - } - - - public bool GetRawButtonReleased(int button) - { - return m_ds.GetStickButtonReleased(Port, button); - } - - public double GetRawAxis(int axis) - { - return m_ds.GetStickAxis(Port, axis); - } - - public int GetPOV(int pov = 0) - { - return m_ds.GetStickPOV(Port, pov); - } - - public int AxisCount => m_ds.GetStickAxisCount(Port); - public int POVCount => m_ds.GetStickPOVCount(Port); - public int ButtonCount => m_ds.GetStickButtonCount(Port); - - public HIDType Type => (HIDType)m_ds.GetJoystickType(Port); - - public string Name => m_ds.GetJoystickName(Port); - - public int Port { get; } - - public void SetOutput(int outputNumber, bool value) - { - - m_outputs = (m_outputs & ~(1 << (outputNumber - 1))) | ((value ? 1 : 0) << (outputNumber - 1)); - Hal.DriverStationLowLevel.SetJoystickOutputs(Port, m_outputs, m_leftRumble, m_rightRumble); - } - - public void SetOutputs(int value) - { - m_outputs = value; - Hal.DriverStationLowLevel.SetJoystickOutputs(Port, m_outputs, m_leftRumble, m_rightRumble); - } - - public void SetRumble(RumbleType type, double value) - { - if (value < 0) - { - value = 0; - } - else if (value > 1) - { - value = 1; - } - if (type == RumbleType.kLeftRumble) - { - m_leftRumble = (short)(value * 65535); - } - else - { - m_rightRumble = (short)(value * 65535); - } - Hal.DriverStationLowLevel.SetJoystickOutputs(Port, m_outputs, m_leftRumble, m_rightRumble); - } - } -} diff --git a/src/wpilibsharp/Geometry/Pose2d.cs b/src/wpilibsharp/Geometry/Pose2d.cs deleted file mode 100644 index 1e1267a7..00000000 --- a/src/wpilibsharp/Geometry/Pose2d.cs +++ /dev/null @@ -1,75 +0,0 @@ -using System; -using UnitsNet; - -namespace WPILib.Geometry -{ - public readonly struct Pose2d : IEquatable - { - public Translation2d Translation { get; } - public Rotation2d Rotation { get; } - - public Pose2d(in Translation2d translation, in Rotation2d rotation) - { - Translation = translation; - Rotation = rotation; - } - - public Pose2d(Length x, Length y, Rotation2d rotation) - { - Translation = new Translation2d(x, y); - Rotation = rotation; - } - - public static Pose2d operator +(in Pose2d left, in Transform2d right) - { - return left.TransformBy(right); - } - - public static Transform2d operator -(in Pose2d left, in Pose2d right) - { - var pose = left.RelativeTo(right); - return new Transform2d(pose.Translation, pose.Rotation); - } - - public static bool operator ==(in Pose2d left, in Pose2d right) - { - return left.Translation == right.Translation && left.Rotation == right.Rotation; - } - - public static bool operator !=(in Pose2d left, in Pose2d right) - { - return left.Translation != right.Translation || left.Rotation != right.Rotation; - } - - public Pose2d TransformBy(in Transform2d other) - { - throw new NotImplementedException(); - } - - public Pose2d RelativeTo(in Pose2d other) - { - throw new NotImplementedException(); - } - - public Pose2d Exp(in Twist2d twist) - { - throw new NotImplementedException(); - } - - public bool Equals(Pose2d other) - { - return this == other; - } - - public override bool Equals(object? other) - { - return other is Pose2d p && this == p; - } - - public override int GetHashCode() - { - return 0; - //return HashCode.Combine(Translation, Rotation); - } - } -} diff --git a/src/wpilibsharp/Geometry/Rotation2d.cs b/src/wpilibsharp/Geometry/Rotation2d.cs deleted file mode 100644 index 7b6e0e40..00000000 --- a/src/wpilibsharp/Geometry/Rotation2d.cs +++ /dev/null @@ -1,118 +0,0 @@ -using System; -using UnitsNet; - -namespace WPILib.Geometry -{ - public readonly struct Rotation2d : IEquatable, IComparable - { - public Angle Angle { get; } - public double Cos { get; } - public double Sin { get; } - - - public double Tan => Sin / Cos; - - public Rotation2d(Angle value) - { - Angle = value; - Cos = Math.Cos(Angle.Radians); - Sin = Math.Sin(Angle.Radians); - } - - public Rotation2d(double x, double y) - { - - var magnitude = Math.Sqrt(x * x + y * y); - if (magnitude > 1e-6) - { - Sin = y / magnitude; - Cos = x / magnitude; - } - else - { - Sin = 0; - Cos = 1; - } - Angle = Angle.FromRadians(Math.Atan2(Sin, Cos)); - - } - - public Rotation2d RotateBy(in Rotation2d other) - { - return new Rotation2d(Cos * other.Cos - Sin * other.Sin, Cos * other.Sin + Sin * other.Cos); - } - - public override bool Equals(object? obj) - { - return obj is Rotation2d d && - Angle == d.Angle; - } - - public override int GetHashCode() - { - return 0; - //return HashCode.Combine(Angle.Degrees); - } - - public bool Equals(Rotation2d other) - { - return Angle == other.Angle; - } - - public int CompareTo(Rotation2d other) - { - return Angle.CompareTo(other.Angle); - } - - public static bool operator >(in Rotation2d left, in Rotation2d right) - { - return left.Angle > right.Angle; - } - - public static bool operator >=(in Rotation2d left, in Rotation2d right) - { - return left.Angle >= right.Angle; - } - - public static bool operator <(in Rotation2d left, in Rotation2d right) - { - return left.Angle < right.Angle; - } - - public static bool operator <=(in Rotation2d left, in Rotation2d right) - { - return left.Angle <= right.Angle; - } - - - - public static Rotation2d operator +(in Rotation2d left, in Rotation2d right) - { - return left.RotateBy(right); - } - - public static Rotation2d operator -(in Rotation2d left, in Rotation2d right) - { - return left + -right; - } - - public static Rotation2d operator -(in Rotation2d left) - { - return new Rotation2d(-left.Angle); - } - - public static Rotation2d operator *(in Rotation2d value, double scalar) - { - return new Rotation2d(value.Angle * scalar); - } - - public static bool operator ==(in Rotation2d left, in Rotation2d right) - { - return left.Angle == right.Angle; - } - public static bool operator !=(in Rotation2d left, in Rotation2d right) - { - return left.Angle != right.Angle; - } - } -} diff --git a/src/wpilibsharp/Geometry/Transform2d.cs b/src/wpilibsharp/Geometry/Transform2d.cs deleted file mode 100644 index 99fbc53e..00000000 --- a/src/wpilibsharp/Geometry/Transform2d.cs +++ /dev/null @@ -1,52 +0,0 @@ -using System; - -namespace WPILib.Geometry -{ - public readonly struct Transform2d : IEquatable - { - public Translation2d Translation { get; } - public Rotation2d Rotation { get; } - - public Transform2d(in Pose2d initial, in Pose2d final) - { - throw new NotImplementedException(); - } - - public Transform2d(in Translation2d translation, in Rotation2d rotation) - { - Translation = translation; - Rotation = rotation; - } - - public static Transform2d operator *(in Transform2d other, double scalar) - { - return new Transform2d(other.Translation * scalar, other.Rotation * scalar); - } - - public static bool operator ==(in Transform2d left, in Transform2d right) - { - return left.Equals(right); - } - - public static bool operator !=(in Transform2d left, in Transform2d right) - { - return !(left == right); - } - - public override bool Equals(object? other) - { - return other is Transform2d t && this == t; - } - - - public bool Equals(Transform2d other) - { - return this == other; - } - - public override int GetHashCode() - { - return 0; // HashCode.Combine(Translation, Rotation); - } - } -} diff --git a/src/wpilibsharp/Geometry/Translation2d.cs b/src/wpilibsharp/Geometry/Translation2d.cs deleted file mode 100644 index dcc62eb0..00000000 --- a/src/wpilibsharp/Geometry/Translation2d.cs +++ /dev/null @@ -1,94 +0,0 @@ -using System; -using UnitsNet; - -namespace WPILib.Geometry -{ - public readonly struct Translation2d : IEquatable - { - public Length X { get; } - public Length Y { get; } - - public Translation2d(Length x, Length y) - { - X = x; - Y = y; - } - - public Length Distance(in Translation2d other) - { - var left = other.X - X; - var right = other.Y - Y; - - var leftArea = left * left; - var rightArea = right * right; - var combined = leftArea + rightArea; - return Length.FromMeters(Math.Sqrt(combined.SquareMeters)); - } - - public Length Norm() - { - - var leftArea = X * X; - var rightArea = Y * Y; - var combined = leftArea + rightArea; - return Length.FromMeters(Math.Sqrt(combined.SquareMeters)); - } - - public static Translation2d operator *(in Translation2d other, double scalar) - { - throw new NotImplementedException(); - } - - public static Translation2d operator /(in Translation2d other, double scalar) - { - throw new NotImplementedException(); - } - - public Translation2d RotateBy(in Rotation2d other) - { - return new Translation2d(X * other.Cos - Y * other.Sin, - X * other.Sin + Y * other.Cos); - } - - public static Translation2d operator +(in Translation2d left, in Translation2d right) - { - return new Translation2d(left.X + right.X, left.Y + right.Y); - } - - public static Translation2d operator -(in Translation2d left, in Translation2d right) - { - return new Translation2d(left.X - right.X, left.Y - right.Y); - } - - public static Translation2d operator -(in Translation2d other) - { - throw new NotImplementedException(); - } - - public static bool operator ==(in Translation2d left, in Translation2d right) - { - return left.Equals(right); - } - - public static bool operator !=(in Translation2d left, in Translation2d right) - { - return !(left == right); - } - - - public override bool Equals(object? obj) - { - return obj is Translation2d t && this == t; - } - - public bool Equals(Translation2d other) - { - return this == other; - } - - public override int GetHashCode() - { - return 0; // HashCode.Combine(X, Y); - } - } -} diff --git a/src/wpilibsharp/Geometry/Twist2d.cs b/src/wpilibsharp/Geometry/Twist2d.cs deleted file mode 100644 index 268bad5b..00000000 --- a/src/wpilibsharp/Geometry/Twist2d.cs +++ /dev/null @@ -1,46 +0,0 @@ -using System; -using UnitsNet; - -namespace WPILib.Geometry -{ - public readonly struct Twist2d : IEquatable - { - public Length dX { get; } - public Length dY { get; } - public Angle dTheta { get; } - - public Twist2d(in Length dx, in Length dy, in Angle dtheta) - { - dX = dx; - dY = dy; - dTheta = dtheta; - } - - public static bool operator ==(in Twist2d left, in Twist2d right) - { - return left.dX == right.dX && - left.dY == right.dY && - left.dTheta == right.dTheta; - } - - public static bool operator !=(in Twist2d left, in Twist2d right) - { - return !(left == right); - } - - public bool Equals(Twist2d other) - { - return this == other; - } - - public override int GetHashCode() - { - return 0; // HashCode.Combine(dX, dY, dTheta); - } - - public override bool Equals(object? obj) - { - return obj is Twist2d t && this == t; - } - } -} diff --git a/src/wpilibsharp/GyroBase.cs b/src/wpilibsharp/GyroBase.cs deleted file mode 100644 index 22ff2afc..00000000 --- a/src/wpilibsharp/GyroBase.cs +++ /dev/null @@ -1,33 +0,0 @@ -using System; -using UnitsNet; -using WPILib.Interfaces; -using WPILib.SmartDashboardNS; - -namespace WPILib -{ -#pragma warning disable CA1063 // Implement IDisposable Correctly - public abstract class GyroBase : IGyro, ISendable -#pragma warning restore CA1063 // Implement IDisposable Correctly - { - public abstract Angle Angle { get; } - public abstract RotationalSpeed Rate { get; } - - public abstract void Calibrate(); -#pragma warning disable CA1063 // Implement IDisposable Correctly - public abstract void Dispose(); -#pragma warning restore CA1063 // Implement IDisposable Correctly - - public virtual void InitSendable(ISendableBuilder builder) - { - if (builder == null) - { - throw new ArgumentNullException(nameof(builder)); - } - - builder.SmartDashboardType = "Gyro"; - builder.AddDoubleProperty("Value", () => Angle.Degrees, null); - } - - public abstract void Reset(); - } -} diff --git a/src/wpilibsharp/IDigitalSource.cs b/src/wpilibsharp/IDigitalSource.cs deleted file mode 100644 index 3870ee62..00000000 --- a/src/wpilibsharp/IDigitalSource.cs +++ /dev/null @@ -1,13 +0,0 @@ -using Hal; - -namespace WPILib -{ - public interface IDigitalSource - { - bool IsAnalogTrigger { get; } - int Channel { get; } - - AnalogTriggerType AnalogTriggerTypeForRouting { get; } - int PortHandleForRouting { get; } - } -} diff --git a/src/wpilibsharp/ISpeedController.cs b/src/wpilibsharp/ISpeedController.cs deleted file mode 100644 index 84738ab9..00000000 --- a/src/wpilibsharp/ISpeedController.cs +++ /dev/null @@ -1,34 +0,0 @@ -using System; -using UnitsNet; - -namespace WPILib -{ - public static class SpeedControllerExtensions - { - public static void SetVoltage(this ISpeedController @this, ElectricPotential potential) - { - if (@this == null) - { - throw new ArgumentNullException(nameof(@this)); - } - - @this.Set(potential / RobotController.BatteryVoltage); - } - } - - public interface ISpeedController - { -#pragma warning disable CA1716 // Identifiers should not match keywords - void Set(double speed); - - double Get(); -#pragma warning restore CA1716 // Identifiers should not match keywords - - bool Inverted { get; set; } - - void Disable(); - - void StopMotor(); - - } -} diff --git a/src/wpilibsharp/Interfaces/IAccelerometer.cs b/src/wpilibsharp/Interfaces/IAccelerometer.cs deleted file mode 100644 index 5ffdbddc..00000000 --- a/src/wpilibsharp/Interfaces/IAccelerometer.cs +++ /dev/null @@ -1,13 +0,0 @@ -using Hal; -using UnitsNet; - -namespace WPILib.Interfaces -{ - public interface IAccelerometer - { - AccelerometerRange Range { set; } - Acceleration X { get; } - Acceleration Y { get; } - Acceleration Z { get; } - } -} diff --git a/src/wpilibsharp/Interfaces/IGyro.cs b/src/wpilibsharp/Interfaces/IGyro.cs deleted file mode 100644 index 53755779..00000000 --- a/src/wpilibsharp/Interfaces/IGyro.cs +++ /dev/null @@ -1,14 +0,0 @@ -using System; -using UnitsNet; - -namespace WPILib.Interfaces -{ - public interface IGyro : IDisposable - { - void Calibrate(); - void Reset(); - Angle Angle { get; } - - RotationalSpeed Rate { get; } - } -} diff --git a/src/wpilibsharp/Interfaces/IPotentiometer.cs b/src/wpilibsharp/Interfaces/IPotentiometer.cs deleted file mode 100644 index 0e70d052..00000000 --- a/src/wpilibsharp/Interfaces/IPotentiometer.cs +++ /dev/null @@ -1,9 +0,0 @@ -using UnitsNet; - -namespace WPILib.Interfaces -{ - public interface IPotentiometer - { - Angle Angle { get; } - } -} diff --git a/src/wpilibsharp/IterativeRobotBase.cs b/src/wpilibsharp/IterativeRobotBase.cs deleted file mode 100644 index d602ea89..00000000 --- a/src/wpilibsharp/IterativeRobotBase.cs +++ /dev/null @@ -1,177 +0,0 @@ -using System; - -namespace WPILib -{ - public abstract class IterativeRobotBase : RobotBase - { - public virtual void RobotInit() - { - Console.WriteLine("Default RobotInit() method... Override me!"); - } - - public virtual void DisabledInit() - { - Console.WriteLine("Default DisabledInit() method... Override me!"); - } - - public virtual void AutonomousInit() - { - Console.WriteLine("Default AutonomousInit() method... Override me!"); - } - - public virtual void TeleopInit() - { - Console.WriteLine("Default TeleopInit() method... Override me!"); - } - - public virtual void TestInit() - { - Console.WriteLine("Default TestInit() method... Override me!"); - } - - public virtual void RobotPeriodic() - { - if (m_rpFirstRun) - { - Console.WriteLine("Default RobotPeriodic() method... Override me!"); - m_rpFirstRun = false; - } - - } - - private bool m_rpFirstRun = true; - private bool m_dpFirstRun = true; - private bool m_apFirstRun = true; - private bool m_tpFirstRun = true; - private bool m_tmpFirstRun = true; - - public virtual void DisabledPeriodic() - { - if (m_dpFirstRun) - { - Console.WriteLine("Default DisabledPeriodic() method... Override me!"); - m_dpFirstRun = false; - } - } - - public virtual void AutonomousPeriodic() - { - if (m_apFirstRun) - { - Console.WriteLine("Default AutonomousPeriodic() method... Override me!"); - m_apFirstRun = false; - } - } - - public virtual void TeleopPeriodic() - { - if (m_tpFirstRun) - { - Console.WriteLine("Default TeleopPeriodic() method... Override me!"); - m_tpFirstRun = false; - } - } - - public virtual void TestPeriodic() - { - if (m_tmpFirstRun) - { - Console.WriteLine("Default TestPeriodic() method... Override me!"); - m_tmpFirstRun = false; - } - } - - public IterativeRobotBase(TimeSpan period) - { - Period = period; - m_watchdog = new Watchdog(period, PrintLoopOverrunMessage); - } - - protected void LoopFunc() - { - m_watchdog.Reset(); - - - if (IsDisabled) - { - if (m_lastMode != Mode.Disabled) - { - DisabledInit(); - m_watchdog.AddEpoch("DisabledInit()"); - m_lastMode = Mode.Disabled; - } - - Hal.DriverStationLowLevel.ObserveUserProgramDisabled(); - DisabledPeriodic(); - m_watchdog.AddEpoch("DisabledPeriodic()"); - } - else if (IsAutonomous) - { - if (m_lastMode != Mode.Autonomous) - { - AutonomousInit(); - m_watchdog.AddEpoch("AutonomousInit()"); - m_lastMode = Mode.Autonomous; - } - - Hal.DriverStationLowLevel.ObserveUserProgramAutonomous(); - AutonomousPeriodic(); - m_watchdog.AddEpoch("AutonomousPeriodic()"); - } - else if (IsOperatorControl) - { - if (m_lastMode != Mode.Teleop) - { - TeleopInit(); - m_watchdog.AddEpoch("TeleopInit()"); - m_lastMode = Mode.Teleop; - } - - Hal.DriverStationLowLevel.ObserveUserProgramTeleop(); - TeleopPeriodic(); - m_watchdog.AddEpoch("TeleopPeriodic()"); - } - else - { - if (m_lastMode != Mode.Test) - { - TestInit(); - m_watchdog.AddEpoch("TestInit()"); - m_lastMode = Mode.Autonomous; - } - - Hal.DriverStationLowLevel.ObserveUserProgramAutonomous(); - TestPeriodic(); - m_watchdog.AddEpoch("TestPeriodic()"); - } - - RobotPeriodic(); - m_watchdog.AddEpoch("RobotPeriodic()"); - m_watchdog.Disable(); - - if (m_watchdog.IsExpired) - { - m_watchdog.PrintEpochs(); - } - } - - public TimeSpan Period { get; protected set; } - - private enum Mode - { - None, - Disabled, - Autonomous, - Teleop, - Test - } - - private Mode m_lastMode = Mode.None; - private readonly Watchdog m_watchdog; - - private void PrintLoopOverrunMessage() - { - DriverStation.ReportWarning($"Loop time of {Period}s overrun\n", false); - } - } -} diff --git a/src/wpilibsharp/LiveWindowNS/LiveWindow.cs b/src/wpilibsharp/LiveWindowNS/LiveWindow.cs deleted file mode 100644 index 12b0c6b9..00000000 --- a/src/wpilibsharp/LiveWindowNS/LiveWindow.cs +++ /dev/null @@ -1,117 +0,0 @@ -using NetworkTables; -using System; -using WPILib.SmartDashboardNS; - -namespace WPILib.LiveWindowNS -{ - public static class LiveWindow - { - private class Component - { - public bool FirstTime { get; set; } - public bool TelemetryEnabled { get; set; } - } - - private static readonly SendableRegistry registry = SendableRegistry.Instance; - private static readonly int dataHandle = registry.DataHandle; - - private static readonly NetworkTable liveWindowTable = NetworkTableInstance.Default.GetTable("LiveWindow"); - private static readonly NetworkTable statusTable = liveWindowTable.GetSubTable(".status"); - private static readonly NetworkTableEntry enabledEntry = statusTable.GetEntry("LW Enabled"); - private static readonly object m_lockObject = new object(); - - private static bool startLiveWindow; - private static bool liveWindowEnabled; - //private static bool telemetryEnabled; - - private static Action? enabledListener; - private static Action? disabledListener; - - private static Component GetOrAdd(ISendable sendable) - { - Component? data = (Component?)registry.GetData(sendable, dataHandle); - if (data == null) - { - data = new Component(); - registry.SetData(sendable, dataHandle, data); - } - return data; - } - - public static Action? EnabledListener - { - get - { - lock (m_lockObject) - { - return enabledListener; - } - } - - set - { - lock (m_lockObject) - { - enabledListener = value; - } - } - } - - public static Action? DisabledListener - { - get - { - lock (m_lockObject) - { - return disabledListener; - } - } - - set - { - lock (m_lockObject) - { - disabledListener = value; - } - } - } - - public static bool Enabled - { - get - { - lock (m_lockObject) - { - return liveWindowEnabled; - } - } - set - { - lock (m_lockObject) - { - if (liveWindowEnabled != value) - { - startLiveWindow = value; - liveWindowEnabled = value; - UpdateValues(); - if (value) - { - Console.WriteLine("Starting live window mode."); - enabledListener?.Invoke(); - } - else - { - Console.WriteLine("Stopping live window mode."); - } - } - } - enabledEntry.SetBoolean(value); - } - } - - private static void UpdateValues() - { - - } - } -} diff --git a/src/wpilibsharp/MotorSafety.cs b/src/wpilibsharp/MotorSafety.cs deleted file mode 100644 index f97e4065..00000000 --- a/src/wpilibsharp/MotorSafety.cs +++ /dev/null @@ -1,118 +0,0 @@ -using System; -using System.Collections.Generic; - -namespace WPILib -{ - public abstract class MotorSafety - { - public static readonly TimeSpan DefaultSafetyExpiration = TimeSpan.FromMilliseconds(100); - - private TimeSpan m_expiration = DefaultSafetyExpiration; - private bool m_enabled; - private TimeSpan m_stopTime = Timer.FPGATimestamp; - private readonly object m_thisMutex = new object(); - private static readonly HashSet m_instanceList = new HashSet(); - private static readonly object m_listMutex = new object(); - - public MotorSafety() - { - lock (m_listMutex) - { - m_instanceList.Add(this); - } - } - - public void Feed() - { - lock (m_thisMutex) - { - m_stopTime = Timer.FPGATimestamp + m_expiration; - } - } - - public TimeSpan Expiration - { - get - { - lock (m_thisMutex) - { - return m_expiration; - } - } - set - { - lock (m_thisMutex) - { - m_expiration = value; - } - } - } - - public bool IsAlive - { - get - { - lock (m_thisMutex) - { - return !m_enabled || m_stopTime > Timer.FPGATimestamp; - } - } - } - - public void Check() - { - bool enabled; - TimeSpan stopTime; - - lock (m_thisMutex) - { - enabled = m_enabled; - stopTime = m_stopTime; - } - - if (!enabled || RobotState.IsDisabled || RobotState.IsTest) - { - return; - } - - if (stopTime < Timer.FPGATimestamp) - { - DriverStation.ReportError(Description + "... Output not updated often enough", false); - StopMotor(); - } - } - - public bool SafetyEnabled - { - get - { - lock (m_thisMutex) - { - return m_enabled; - } - } - set - { - lock (m_thisMutex) - { - m_enabled = value; - } - } - } - - public static void CheckMotors() - { - lock (m_listMutex) - { - foreach (var elem in m_instanceList) - { - elem.Check(); - } - } - } - - public abstract void StopMotor(); - - public abstract string Description { get; } - } -} diff --git a/src/wpilibsharp/Notifier.cs b/src/wpilibsharp/Notifier.cs deleted file mode 100644 index 5379747a..00000000 --- a/src/wpilibsharp/Notifier.cs +++ /dev/null @@ -1,123 +0,0 @@ -using System; -using System.Threading; - -namespace WPILib -{ - public sealed class Notifier : IDisposable - { - private readonly Thread m_thread; - private readonly object m_processMutex = new object(); - private int m_notifier; - private Action m_handler; - private TimeSpan m_expirationTime; - private TimeSpan m_period; - private bool m_periodic; - - - public Notifier(Action handler) - { - m_handler = handler; - m_notifier = Hal.NotifierLowLevel.Initialize(); - - m_thread = new Thread(() => - { - while (true) - { - var notifier = m_notifier; - if (notifier == 0) break; - var curTime = Hal.NotifierLowLevel.WaitForAlarm(m_notifier, out var status); - if (curTime == 0 || status != 0) break; - - Action handler; - lock (m_processMutex) - { - handler = m_handler; - if (m_periodic) - { - m_expirationTime += m_period; - UpdateAlarm(); - } - else - { - UpdateAlarm(ulong.MaxValue); - } - } - handler?.Invoke(); - } - }); - - m_thread.Start(); - } - - public string Name - { - set - { - if (m_thread.Name == null) - { - m_thread.Name = value; - } - Hal.NotifierLowLevel.SetName(m_notifier, value); - } - } - - public Action Handler - { - set - { - lock (m_processMutex) - { - m_handler = value; - } - } - } - - public void StartSingle(TimeSpan time) - { - lock (m_processMutex) - { - m_periodic = false; - m_expirationTime = Timer.FPGATimestamp + time; - UpdateAlarm(); - } - } - - public void StartPeriodic(TimeSpan time) - { - lock (m_processMutex) - { - m_periodic = true; - m_period = time; - m_expirationTime = Timer.FPGATimestamp + time; - UpdateAlarm(); - } - } - - public void Stop() - { - Hal.NotifierLowLevel.CancelAlarm(m_notifier); - } - - private void UpdateAlarm() - { - UpdateAlarm((ulong)(m_expirationTime.TotalMilliseconds * 1000)); - } - - private void UpdateAlarm(ulong triggerTime) - { - var notifier = m_notifier; - if (notifier == 0) return; - - Hal.NotifierLowLevel.UpdateAlarm(notifier, triggerTime); - } - - public void Dispose() - { - var handle = Interlocked.Exchange(ref m_notifier, 0); - Hal.NotifierLowLevel.Stop(handle); - m_thread.Join(); - - Hal.NotifierLowLevel.Clean(handle); - } - } -} diff --git a/src/wpilibsharp/PWM.cs b/src/wpilibsharp/PWM.cs deleted file mode 100644 index e247c216..00000000 --- a/src/wpilibsharp/PWM.cs +++ /dev/null @@ -1,142 +0,0 @@ -using Hal; -using System; -using System.Globalization; -using WPILib.SmartDashboardNS; - -namespace WPILib -{ -#pragma warning disable CA1063 // Implement IDisposable Correctly - public class PWM : MotorSafety, ISendable, IDisposable -#pragma warning restore CA1063 // Implement IDisposable Correctly - { - public enum PeriodMultiplierType - { - k1X, - k2X, - k4X - } - - private readonly int m_channel; - internal int m_handle; - - public PWM(int channel) - { - m_channel = channel; - m_handle = Hal.PWMLowLevel.InitializePort(Hal.HALLowLevel.GetPort(channel)); - - SetDisabled(); - - Hal.PWMLowLevel.SetEliminateDeadband(m_handle, false); - - UsageReporting.Report(ResourceType.PWM, channel + 1); - - SendableRegistry.Instance.AddLW(this, "PWM", channel); - - SafetyEnabled = false; - } - -#pragma warning disable CA1063 // Implement IDisposable Correctly - public virtual void Dispose() -#pragma warning restore CA1063 // Implement IDisposable Correctly - { - SendableRegistry.Instance.Remove(this); - if (m_handle == 0) return; - SetDisabled(); - Hal.PWMLowLevel.FreePort(m_handle); - m_handle = 0; - GC.SuppressFinalize(this); - } - - public override void StopMotor() - { - SetDisabled(); - } - - public void SetBounds(double max, double deadbandMax, double center, double deadbandMin, double min) - { - Hal.PWMLowLevel.SetConfig(m_handle, max, deadbandMax, center, deadbandMin, min); - } - - public override string Description => $"PWM {Channel.ToString(CultureInfo.InvariantCulture)}"; - - public int Channel => m_channel; - - public double Position - { - get - { - return Hal.PWMLowLevel.GetPosition(m_handle); - } - set - { - Hal.PWMLowLevel.SetPosition(m_handle, value); - } - } - - public double Speed - { - get - { - return Hal.PWMLowLevel.GetSpeed(m_handle); - } - set - { - Hal.PWMLowLevel.SetSpeed(m_handle, value); - } - } - - public int Raw - { - get - { - return Hal.PWMLowLevel.GetRaw(m_handle); - } - set - { - Hal.PWMLowLevel.SetRaw(m_handle, value); - } - } - - public void SetDisabled() - { - Hal.PWMLowLevel.SetDisabled(m_handle); - } - - public PeriodMultiplierType PeriodMultiplier - { - set - { - switch (value) - { - case PeriodMultiplierType.k1X: - Hal.PWMLowLevel.SetPeriodScale(m_handle, 0); - break; - case PeriodMultiplierType.k2X: - Hal.PWMLowLevel.SetPeriodScale(m_handle, 1); - break; - case PeriodMultiplierType.k4X: - Hal.PWMLowLevel.SetPeriodScale(m_handle, 3); - break; - } - } - } - - public void SetZeroLatch() - { - Hal.PWMLowLevel.LatchZero(m_handle); - } - - public virtual void InitSendable(ISendableBuilder builder) - { - if (builder == null) - { - throw new ArgumentNullException(nameof(builder)); - } - - builder.SmartDashboardType = "PWM"; - builder.IsActuator = true; - builder.SafeState = SetDisabled; - builder.AddDoubleProperty("Value", () => Raw, v => Raw = (int)v); - } - } -} diff --git a/src/wpilibsharp/PWMSparkMax.cs b/src/wpilibsharp/PWMSparkMax.cs deleted file mode 100644 index 23d488bd..00000000 --- a/src/wpilibsharp/PWMSparkMax.cs +++ /dev/null @@ -1,23 +0,0 @@ -using Hal; -using UnitsNet; -using WPILib.SmartDashboardNS; - -namespace WPILib -{ - public sealed class PWMSparkMax : PWMSpeedController - { - public PWMSparkMax(int channel) : base(channel) - { - SetBounds(2.003, 1.55, 1.50, 1.46, 0.999); - PeriodMultiplier = PeriodMultiplierType.k1X; - Speed = 0.0; - SetZeroLatch(); - - UsageReporting.Report(ResourceType.RevSparkMaxPWM, Channel + 1); - SendableRegistry.Instance.SetName(this, "PWMSparkMax", Channel); - - ElectricPotential.FromVolts(5.0); - //this.SetVoltage(ElecticPotential.) - } - } -} diff --git a/src/wpilibsharp/PWMSpeedController.cs b/src/wpilibsharp/PWMSpeedController.cs deleted file mode 100644 index e9238987..00000000 --- a/src/wpilibsharp/PWMSpeedController.cs +++ /dev/null @@ -1,47 +0,0 @@ -using System; -using System.Globalization; -using WPILib.SmartDashboardNS; - -namespace WPILib -{ - public abstract class PWMSpeedController : PWM, ISpeedController, ISendable - { - protected PWMSpeedController(int channel) : base(channel) - { - - } - - public override string Description => $"PWM {Channel.ToString(CultureInfo.InvariantCulture)}"; - - public bool Inverted { get; set; } - - public void Set(double speed) - { - Speed = Inverted ? -speed : speed; - Feed(); - } - - public double Get() - { - return Speed; - } - - public void Disable() - { - SetDisabled(); - } - - public override void InitSendable(ISendableBuilder builder) - { - if (builder == null) - { - throw new ArgumentNullException(nameof(builder)); - } - - builder.SmartDashboardType = "Speed Controller"; - builder.IsActuator = true; - builder.SafeState = SetDisabled; - builder.AddDoubleProperty("Value", Get, Set); - } - } -} diff --git a/src/wpilibsharp/RobotBase.cs b/src/wpilibsharp/RobotBase.cs deleted file mode 100644 index aa1ccad4..00000000 --- a/src/wpilibsharp/RobotBase.cs +++ /dev/null @@ -1,215 +0,0 @@ -using Hal; -using NetworkTables; -using System; -using System.IO; -using System.Threading; -using WPILib.LiveWindowNS; - -namespace WPILib -{ -#pragma warning disable CA1063 // Implement IDisposable Correctly - public abstract class RobotBase : IDisposable -#pragma warning restore CA1063 // Implement IDisposable Correctly - { - private static void RunRobot() where Robot : RobotBase, new() - { - Console.WriteLine("\n********** Robot program starting **********"); - - Robot robot; - try - { - robot = new Robot(); - } -#pragma warning disable CA1031 // Do not catch general exception types - catch (Exception ex) -#pragma warning restore CA1031 // Do not catch general exception types - { - string robotName = typeof(Robot).Name; - DriverStation.ReportError(("Unhandled exception instanciating robot " - + robotName + " " + ex.Message), ex.StackTrace!); - DriverStation.ReportWarning("Robots should not quit, but yours did!", false); - DriverStation.ReportError("Could not instanciate robot " + robotName + "!", false); - return; - } - - lock (m_runMutex) - { - m_robotCopy = robot; - } - - if (IsReal) - { - try - { - File.WriteAllText("/tmp/frc_versions/FRC_Lib_Version.ini", $"C# {WPILibVersion}"); - } - catch (IOException ex) - { - DriverStation.ReportError("Could not write FRC_Lib_Version.ini: " + ex.Message, ex.StackTrace!); - } - } - - bool errorOnExit = false; - try - { - robot.StartCompetition(); - } -#pragma warning disable CA1031 // Do not catch general exception types - catch (Exception ex) -#pragma warning restore CA1031 // Do not catch general exception types - { - Console.WriteLine(ex); - DriverStation.ReportError("Unhandled Exception: " + ex.Message, ex.StackTrace!); - errorOnExit = true; - } - finally - { - bool suppressExitWarningLocal; - lock (m_runMutex) - { - suppressExitWarningLocal = suppressExitWarning; - } - - if (!suppressExitWarningLocal) - { - DriverStation.ReportWarning("Robots should not quit, but yours did!", false); - if (errorOnExit) - { - DriverStation.ReportError("The StartCompetition() method (or methods called by it) should have handled the exception above.", false); - } - else - { - DriverStation.ReportError("Unexpected return from StartCompetition() method.", false); - } - } - } - } - - private static bool suppressExitWarning = false; - - private static int RunHALInitialization() - { - if (!Hal.HALLowLevel.Initialize()) - { - Console.WriteLine("FATAL ERROR: HAL could not be initialized"); - return -1; - } - - NetworkTables.Natives.NtCore.Initialize(); - - return 0; - } - - private static readonly object m_runMutex = new object(); - private static RobotBase? m_robotCopy; - public static void SuppressExitWarning(bool value) - { - lock (m_runMutex) - { - suppressExitWarning = value; - } - } - - public static void StartRobot() where TRobot : RobotBase, new() - { - int halInit = RunHALInitialization(); - if (halInit != 0) - { - throw new InvalidOperationException("Base Robot Functionality Failed to Initialize. Terminating"); - } - - UsageReporting.Report(ResourceType.Language, Instances.Language_DotNet, 0, WPILibVersion); - - if (MainLowLevel.HasMain()) - { - Thread thread = new Thread(() => - { - RunRobot(); - MainLowLevel.ExitMain(); - }) - { - Name = "Robot Main", - IsBackground = true - }; - thread.Start(); - MainLowLevel.RunMain(); - SuppressExitWarning(true); - RobotBase? robot; - lock (m_runMutex) - { - robot = m_robotCopy; - } - - robot?.EndCompetition(); - if (thread.Join(TimeSpan.FromSeconds(1))) - { - robot?.Dispose(); - } - } - else - { - RunRobot(); - } - - - } - - protected DriverStation DriverStation { get; } - - protected static int ThreadId { get; set; } - - public bool IsEnabled => DriverStation.IsEnabled; - public bool IsDisabled => DriverStation.IsDisabled; - public bool IsAutonomous => DriverStation.IsAutonomous; - public bool IsOperatorControl => DriverStation.IsOperatorControl; - public bool IsTest => DriverStation.IsTest; - - public bool IsNewDataAvailable => DriverStation.IsNewControlData; - - public static int MainThreadId => ThreadId; - - public static string WPILibVersion => "1234"; - - public abstract void StartCompetition(); - - public abstract void EndCompetition(); - -#pragma warning disable CA1063 // Implement IDisposable Correctly - public virtual void Dispose() -#pragma warning restore CA1063 // Implement IDisposable Correctly - { - GC.SuppressFinalize(this); - } - - public static bool IsReal => Hal.HALLowLevel.GetRuntimeType() == Hal.RuntimeType.Athena; - public static bool IsSimulation => !IsReal; - - public RobotBase() - { - ThreadId = Thread.CurrentThread.ManagedThreadId; - - var inst = NetworkTableInstance.Default; - inst.SetNetworkIdentity("Robot"); - - - if (IsReal) - { - inst.StartServer("/home/lvuser/networktables.ini"); - } - else - { - inst.StartServer(); - } - - inst.GetTable("LiveWindow") - .GetSubTable(".status") - .GetEntry("LW Enabled") - .SetBoolean(false); - - DriverStation = DriverStation.Instance; - LiveWindow.Enabled = false; - - } - - } -} diff --git a/src/wpilibsharp/RobotController.cs b/src/wpilibsharp/RobotController.cs deleted file mode 100644 index c6ddd6d1..00000000 --- a/src/wpilibsharp/RobotController.cs +++ /dev/null @@ -1,11 +0,0 @@ -using UnitsNet; - -namespace WPILib -{ - public static class RobotController - { - public static ElectricPotential BatteryVoltage => ElectricPotential.FromVolts(Hal.PowerLowLevel.GetVinVoltage()); - - public static ElectricPotential Voltage5V => ElectricPotential.FromVolts(Hal.PowerLowLevel.GetUserVoltage5V()); - } -} diff --git a/src/wpilibsharp/RobotRunner.cs b/src/wpilibsharp/RobotRunner.cs new file mode 100644 index 00000000..e328f6e4 --- /dev/null +++ b/src/wpilibsharp/RobotRunner.cs @@ -0,0 +1,5 @@ +namespace WPILib; + +public static class RobotRunner { + +} diff --git a/src/wpilibsharp/RobotState.cs b/src/wpilibsharp/RobotState.cs deleted file mode 100644 index 033270a8..00000000 --- a/src/wpilibsharp/RobotState.cs +++ /dev/null @@ -1,10 +0,0 @@ -namespace WPILib -{ - public static class RobotState - { - private static readonly DriverStation dsInstance = DriverStation.Instance; - - public static bool IsDisabled => dsInstance.IsDisabled; - public static bool IsTest => dsInstance.IsTest; - } -} diff --git a/src/wpilibsharp/ShuffleboardNS/BuiltInLayouts.cs b/src/wpilibsharp/ShuffleboardNS/BuiltInLayouts.cs deleted file mode 100644 index e19cdd58..00000000 --- a/src/wpilibsharp/ShuffleboardNS/BuiltInLayouts.cs +++ /dev/null @@ -1,16 +0,0 @@ -namespace WPILib.ShuffleboardNS -{ - public sealed class BuiltInLayouts : ILayoutType - { - public static BuiltInLayouts List { get; } = new BuiltInLayouts("List Layout"); - - public static BuiltInLayouts Grid { get; } = new BuiltInLayouts("Grid Layout"); - - public string LayoutName { get; } - - internal BuiltInLayouts(string layoutName) - { - LayoutName = layoutName; - } - } -} diff --git a/src/wpilibsharp/ShuffleboardNS/BuiltInWidgets.cs b/src/wpilibsharp/ShuffleboardNS/BuiltInWidgets.cs deleted file mode 100644 index a0b903b6..00000000 --- a/src/wpilibsharp/ShuffleboardNS/BuiltInWidgets.cs +++ /dev/null @@ -1,14 +0,0 @@ -namespace WPILib.ShuffleboardNS -{ - public sealed class BuiltInWidgets : IWidgetType - { - public static BuiltInWidgets TextView { get; } = new BuiltInWidgets("Text View"); - - public string WidgetName { get; } - - internal BuiltInWidgets(string widgetName) - { - WidgetName = widgetName; - } - } -} diff --git a/src/wpilibsharp/ShuffleboardNS/ComplexWidget.cs b/src/wpilibsharp/ShuffleboardNS/ComplexWidget.cs deleted file mode 100644 index f4d6ce45..00000000 --- a/src/wpilibsharp/ShuffleboardNS/ComplexWidget.cs +++ /dev/null @@ -1,47 +0,0 @@ -using NetworkTables; -using WPILib.SmartDashboardNS; - -namespace WPILib.ShuffleboardNS -{ - public sealed class ComplexWidget : ShuffleboardWidget - { - private readonly ISendable m_sendable; - private SendableBuilderImpl? m_builder; - - internal ComplexWidget(IShuffleboardContainer parent, string title, ISendable sendable) - : base(parent, title) - { - - m_sendable = sendable; - } - - public override void BuildInto(NetworkTable parentTable, NetworkTable metaTable) - { - BuildMetadata(metaTable); - if (m_builder == null) - { - m_builder = new SendableBuilderImpl(); - m_builder.Table = parentTable.GetSubTable(Title); - m_sendable.InitSendable(m_builder); - m_builder.StartListeners(); - } - m_builder.TriggerUpdateTable(); - } - - internal void EnableIfActuator() - { - if (m_builder != null && m_builder.IsActuator) - { - m_builder.StartLiveWindowMode(); - } - } - - internal void DisableIfActuator() - { - if (m_builder != null && m_builder.IsActuator) - { - m_builder.StopLiveWindowMode(); - } - } - } -} diff --git a/src/wpilibsharp/ShuffleboardNS/ContainerHelper.cs b/src/wpilibsharp/ShuffleboardNS/ContainerHelper.cs deleted file mode 100644 index 1488f560..00000000 --- a/src/wpilibsharp/ShuffleboardNS/ContainerHelper.cs +++ /dev/null @@ -1,155 +0,0 @@ -using System; -using System.Collections.Generic; -using NetworkTables; -using WPILib.SmartDashboardNS; - -namespace WPILib.ShuffleboardNS -{ - internal sealed class ContainerHelper - { - private readonly IShuffleboardContainer m_container; - private readonly HashSet m_usedTitles = new HashSet(); - private readonly List m_components = new List(); - private readonly Dictionary m_layouts = new Dictionary(); - - internal ContainerHelper(IShuffleboardContainer container) - { - m_container = container; - } - - public List Components => m_components; - - public ShuffleboardLayout GetLayout(string title, string type) - { - if (m_layouts.TryGetValue(title, out var layout)) - { - return layout; - } - - layout = new ShuffleboardLayout(m_container, type, title); - m_components.Add(layout); - m_layouts.Add(title, layout); - return layout; - } - - public ShuffleboardLayout GetLayout(string title) - { - if (m_layouts.TryGetValue(title, out var layout)) - { - return layout; - } - throw new KeyNotFoundException($"No layout has been defined with the title '{title}'"); - } - - public ComplexWidget Add(string title, ISendable sendable) - { - CheckTitle(title); - var widget = new ComplexWidget(m_container, title, sendable); - m_components.Add(widget); - return widget; - } - - public ComplexWidget Add(ISendable sendable) - { - var name = SendableRegistry.Instance.GetName(sendable); - if (string.IsNullOrWhiteSpace(name)) - { - throw new ArgumentOutOfRangeException(nameof(sendable), "Sendable must have a name"); - } - return Add(name, sendable); - } - - public SimpleWidget Add(string title, T defaultValue) - { - CheckTitle(title); - CheckNtType(defaultValue); - - var widget = new SimpleWidget(m_container, title); - m_components.Add(widget); - widget.Entry.SetDefaultValue(defaultValue); - return widget; - } - - public SuppliedValueWidget Add(string title, Func valueSupplier) - { - Precheck(title, valueSupplier); - return AddSupplied(title, valueSupplier, (e, v) => e.SetValue(v)); - } - - public SuppliedValueWidget AddString(string title, Func valueSupplier) - { - Precheck(title, valueSupplier); - return AddSupplied(title, valueSupplier, (e, v) => e.SetString(v)); - } - - public SuppliedValueWidget AddNumber(string title, Func valueSupplier) - { - Precheck(title, valueSupplier); - return AddSupplied(title, valueSupplier, (e, v) => e.SetDouble(v)); - } - - public SuppliedValueWidget AddBoolean(string title, Func valueSupplier) - { - Precheck(title, valueSupplier); - return AddSupplied(title, valueSupplier, (e, v) => e.SetBoolean(v)); - } - - public SuppliedValueWidget AddStringArray(string title, Func valueSupplier) - { - Precheck(title, valueSupplier); - return AddSupplied(title, valueSupplier, (e, v) => e.SetStringArray(v)); - } - - public SuppliedValueWidget AddDoubleArray(string title, Func valueSupplier) - { - Precheck(title, valueSupplier); - return AddSupplied(title, valueSupplier, (e, v) => e.SetDoubleArray(v)); - } - - public SuppliedValueWidget AddBooleanArray(string title, Func valueSupplier) - { - Precheck(title, valueSupplier); - return AddSupplied(title, valueSupplier, (e, v) => e.SetBooleanArray(v)); - } - - public SuppliedValueWidget AddRaw(string title, Func valueSupplier) - { - Precheck(title, valueSupplier); - return AddSupplied(title, valueSupplier, (e, v) => e.SetRaw(v)); - } - - private SuppliedValueWidget AddSupplied(string title, Func supplier, Action setter) - { - var widget = new SuppliedValueWidget(m_container, title, supplier, setter); - m_components.Add(widget); - return widget; - } - - private void Precheck(string title, Func valueSupplier) - { - if (title == null) - { - throw new ArgumentNullException(nameof(title)); - } - if (valueSupplier == null) - { - throw new ArgumentNullException(nameof(valueSupplier)); - } - CheckTitle(title); - } - - private static void CheckNtType(T data) - { - var _ = data; - } - - private void CheckTitle(string title) - { - if (m_usedTitles.Contains(title)) - { - throw new ArgumentException($"Title is already in use: {title}"); - } - m_usedTitles.Add(title); - } - } -} diff --git a/src/wpilibsharp/ShuffleboardNS/EventImportance.cs b/src/wpilibsharp/ShuffleboardNS/EventImportance.cs deleted file mode 100644 index 25a8d052..00000000 --- a/src/wpilibsharp/ShuffleboardNS/EventImportance.cs +++ /dev/null @@ -1,27 +0,0 @@ -namespace WPILib.ShuffleboardNS -{ - public enum EventImportance - { - Trivial, - Low, - Normal, - High, - Critical - } - - public static class EventImportanceExtensions - { - public static string GetName(this EventImportance importance) - { - return importance switch - { - EventImportance.Trivial => "TRIVIAL", - EventImportance.Low => "LOW", - EventImportance.Normal => "NORMAL", - EventImportance.High => "HIGH", - EventImportance.Critical => "CRITICAL", - _ => "NORMAL" - }; - } - } -} diff --git a/src/wpilibsharp/ShuffleboardNS/ILayoutType.cs b/src/wpilibsharp/ShuffleboardNS/ILayoutType.cs deleted file mode 100644 index 8440de30..00000000 --- a/src/wpilibsharp/ShuffleboardNS/ILayoutType.cs +++ /dev/null @@ -1,7 +0,0 @@ -namespace WPILib.ShuffleboardNS -{ - public interface ILayoutType - { - string LayoutName { get; } - } -} diff --git a/src/wpilibsharp/ShuffleboardNS/IShuffleboardContainer.cs b/src/wpilibsharp/ShuffleboardNS/IShuffleboardContainer.cs deleted file mode 100644 index 2d56d71f..00000000 --- a/src/wpilibsharp/ShuffleboardNS/IShuffleboardContainer.cs +++ /dev/null @@ -1,41 +0,0 @@ -using System; -using System.Collections.Generic; -using WPILib.SmartDashboardNS; - -namespace WPILib.ShuffleboardNS -{ - public interface IShuffleboardContainer : IShuffleboardValue - { - List Components { get; } - - ShuffleboardLayout GetLayout(string title, string type); - - ShuffleboardLayout GetLayout(string title, ILayoutType layoutType); - - ShuffleboardLayout GetLayout(string title); - - ComplexWidget Add(string title, ISendable sendable); - - ComplexWidget Add(ISendable sendable); - - SimpleWidget Add(string title, T defaultValue); - - SuppliedValueWidget Add(string title, Func valueSupplier); - - SuppliedValueWidget AddString(string title, Func valueSupplier); - - SuppliedValueWidget AddNumber(string title, Func valueSupplier); - - SuppliedValueWidget AddBoolean(string title, Func valueSupplier); - - SuppliedValueWidget AddStringArray(string title, Func valueSupplier); - - SuppliedValueWidget AddDoubleArray(string title, Func valueSupplier); - - SuppliedValueWidget AddBooleanArray(string title, Func valueSupplier); - - SuppliedValueWidget AddRaw(string title, Func valueSupplier); - - SimpleWidget AddPersistent(string title, T defaultValue); - } -} diff --git a/src/wpilibsharp/ShuffleboardNS/IShuffleboardRoot.cs b/src/wpilibsharp/ShuffleboardNS/IShuffleboardRoot.cs deleted file mode 100644 index 45ac156f..00000000 --- a/src/wpilibsharp/ShuffleboardNS/IShuffleboardRoot.cs +++ /dev/null @@ -1,16 +0,0 @@ -namespace WPILib.ShuffleboardNS -{ - internal interface IShuffleboardRoot - { - ShuffleboardTab GetTab(string title); - - void Update(); - - void EnableActuatorWidgets(); - void DisableActuatorWidgets(); - - void SelectTab(int index); - - void SelectTab(string title); - } -} diff --git a/src/wpilibsharp/ShuffleboardNS/IShuffleboardValue.cs b/src/wpilibsharp/ShuffleboardNS/IShuffleboardValue.cs deleted file mode 100644 index b9f9d209..00000000 --- a/src/wpilibsharp/ShuffleboardNS/IShuffleboardValue.cs +++ /dev/null @@ -1,11 +0,0 @@ -using NetworkTables; - -namespace WPILib.ShuffleboardNS -{ - public interface IShuffleboardValue - { - string Title { get; } - - void BuildInto(NetworkTable parentTable, NetworkTable metaTable); - } -} diff --git a/src/wpilibsharp/ShuffleboardNS/IWidgetType.cs b/src/wpilibsharp/ShuffleboardNS/IWidgetType.cs deleted file mode 100644 index 706d9611..00000000 --- a/src/wpilibsharp/ShuffleboardNS/IWidgetType.cs +++ /dev/null @@ -1,7 +0,0 @@ -namespace WPILib.ShuffleboardNS -{ - public interface IWidgetType - { - string WidgetName { get; } - } -} diff --git a/src/wpilibsharp/ShuffleboardNS/RecordingController.cs b/src/wpilibsharp/ShuffleboardNS/RecordingController.cs deleted file mode 100644 index 2b8a1375..00000000 --- a/src/wpilibsharp/ShuffleboardNS/RecordingController.cs +++ /dev/null @@ -1,54 +0,0 @@ -using NetworkTables; - -namespace WPILib.ShuffleboardNS -{ - internal sealed class RecordingController - { - private const string RecordingTableName = "/Shuffleboard/.recording/"; - private const string RecordingControlKey = RecordingTableName + "RecordData"; - private const string RecordingFileNameFormatKey = RecordingTableName + "FileNameFormat"; - private const string EventMarkerTableName = RecordingTableName + "events"; - - private readonly NetworkTableEntry m_recordingControlEntry; - private readonly NetworkTableEntry m_recordingFileNameFormatEntry; - private readonly NetworkTable m_eventsTable; - - internal RecordingController(NetworkTableInstance ntInstance) - { - m_recordingControlEntry = ntInstance.GetEntry(RecordingControlKey); - m_recordingFileNameFormatEntry = ntInstance.GetEntry(RecordingFileNameFormatKey); - m_eventsTable = ntInstance.GetTable(EventMarkerTableName); - } - - public void StartRecording() - { - m_recordingControlEntry.SetBoolean(true); - } - - public void StopRecording() - { - m_recordingControlEntry.SetBoolean(false); - } - - public void SetRecordingFileNameFormat(string format) - { - m_recordingFileNameFormatEntry.SetString(format); - } - - public void ClearRecordingFileNameFormat() - { - m_recordingFileNameFormatEntry.Delete(); - } - - public void AddEventMarker(string name, string description, EventImportance importance) - { - if (string.IsNullOrWhiteSpace(name)) - { - DriverStation.ReportError("Shuffleboard event name was not specified", true); - return; - } - - m_eventsTable.GetSubTable(name).GetEntry("Info").SetStringArray(new string[] { description, importance.GetName() }); - } - } -} diff --git a/src/wpilibsharp/ShuffleboardNS/Shuffleboard.cs b/src/wpilibsharp/ShuffleboardNS/Shuffleboard.cs deleted file mode 100644 index 0c7f1bf3..00000000 --- a/src/wpilibsharp/ShuffleboardNS/Shuffleboard.cs +++ /dev/null @@ -1,73 +0,0 @@ -using NetworkTables; - -namespace WPILib.ShuffleboardNS -{ - public static class Shuffleboard - { - public static readonly string kBaseTableName = "/Shuffleboard"; - - private static readonly IShuffleboardRoot root = new ShuffleboardInstance(NetworkTableInstance.Default); - private static readonly RecordingController recordingController = new RecordingController(NetworkTableInstance.Default); - - public static void Update() - { - root.Update(); - } - - public static ShuffleboardTab GetTab(string title) - { - return root.GetTab(title); - } - - public static void SelectTab(int index) - { - root.SelectTab(index); - } - - public static void SelectTab(string title) - { - root.SelectTab(title); - } - - public static void EnableActuatorWidgets() - { - root.EnableActuatorWidgets(); - } - - public static void DisableActuatorWidgets() - { - Update(); - root.DisableActuatorWidgets(); - } - - public static void StartRecording() - { - recordingController.StartRecording(); - } - - public static void StopRecording() - { - recordingController.StopRecording(); - } - - public static void SetRecordingFileNameFormat(string format) - { - recordingController.SetRecordingFileNameFormat(format); - } - - public static void ClearRecordingFileNameFormat() - { - recordingController.ClearRecordingFileNameFormat(); - } - - public static void AddEventMarker(string name, string description, EventImportance importance) - { - recordingController.AddEventMarker(name, description, importance); - } - - public static void AddEventMarker(string name, EventImportance importance) - { - AddEventMarker(name, "", importance); - } - } -} diff --git a/src/wpilibsharp/ShuffleboardNS/ShuffleboardComponent.cs b/src/wpilibsharp/ShuffleboardNS/ShuffleboardComponent.cs deleted file mode 100644 index d87f2c45..00000000 --- a/src/wpilibsharp/ShuffleboardNS/ShuffleboardComponent.cs +++ /dev/null @@ -1,141 +0,0 @@ -using System; -using System.Collections.Generic; -using NetworkTables; - -namespace WPILib.ShuffleboardNS -{ - public abstract class ShuffleboardComponent : IShuffleboardValue - { - private string? m_type; - - //protected Dictionary? m_properties; - - protected bool MetadataDirty { get; set; } = true; - - protected ShuffleboardComponent(IShuffleboardContainer parent, string title, string? type = null) - { - Parent = parent; - Title = title; - m_type = type; - } - - public IShuffleboardContainer Parent { get; } - - public string? Type - { - get => m_type; - protected set - { - m_type = value; - MetadataDirty = true; - } - } - - public string Title { get; } - - protected IDictionary Properties { get; } = new Dictionary(); - - protected int Column { get; set; } = -1; - protected int Row { get; set; } = -1; - protected int Width { get; set; } = -1; - - protected int Height { get; set; } = -1; - - public abstract void BuildInto(NetworkTable parentTable, NetworkTable metaTable); - } - - public abstract class ShuffleboardComponent : ShuffleboardComponent where T : ShuffleboardComponent - { - protected ShuffleboardComponent(IShuffleboardContainer parent, string title, string? type = null) - : base(parent, title, type) - { - } - - - public T WithProperties(IDictionary properties) - { - if (properties == null) - { - throw new ArgumentNullException(nameof(properties)); - } - - Properties.Clear(); - foreach (var prop in properties) - { - Properties.Add(prop.Key, prop.Value); - } - MetadataDirty = true; - return (T)this; - } - - public T WithPosition(int columnIndex, int rowIndex) - { - Column = columnIndex; - Row = rowIndex; - MetadataDirty = true; - return (T)this; - } - - public T WithSize(int width, int height) - { - Width = width; - Height = height; - MetadataDirty = true; - return (T)this; - } - - protected void BuildMetadata(NetworkTable metaTable) - { - if (metaTable == null) - { - throw new ArgumentNullException(nameof(metaTable)); - } - - if (!MetadataDirty) return; - - var preferredComponentEntry = metaTable.GetEntry("PreferredComponent"); - - if (Type == null) - { - preferredComponentEntry.Delete(); - } - else - { - preferredComponentEntry.ForceSetString(Type); - } - - var sizeEntry = metaTable.GetEntry("Size"); - - if (Width <= 0 || Height <= 0) - { - sizeEntry.Delete(); - } - else - { - sizeEntry.SetDoubleArray(stackalloc double[] { Width, Height }); - } - - var positionEntry = metaTable.GetEntry("Position"); - if (Column < 0 || Row < 0) - { - positionEntry.Delete(); - } - else - { - positionEntry.SetDoubleArray(stackalloc double[] { Column, Row }); - } - - if (Properties != null) - { - var propTable = metaTable.GetSubTable("Properties"); - foreach (var prop in Properties) - { - propTable.GetEntry(prop.Key).SetValue(prop.Value); - } - } - MetadataDirty = false; - } - - - } -} diff --git a/src/wpilibsharp/ShuffleboardNS/ShuffleboardInstance.cs b/src/wpilibsharp/ShuffleboardNS/ShuffleboardInstance.cs deleted file mode 100644 index 1b1228ad..00000000 --- a/src/wpilibsharp/ShuffleboardNS/ShuffleboardInstance.cs +++ /dev/null @@ -1,96 +0,0 @@ -using System; -using System.Collections.Generic; -using System.Linq; -using Hal; -using NetworkTables; - -namespace WPILib.ShuffleboardNS -{ - internal sealed class ShuffleboardInstance : IShuffleboardRoot - { - private readonly Dictionary m_tabs = new Dictionary(); - - private bool m_tabsChanged = false; - private readonly NetworkTable m_rootTable; - private readonly NetworkTable m_rootMetaTable; - private readonly NetworkTableEntry m_selectedTabEntry; - - internal ShuffleboardInstance(NetworkTableInstance ntInstance) - { - m_rootTable = ntInstance.GetTable(Shuffleboard.kBaseTableName); - m_rootMetaTable = m_rootTable.GetSubTable(".metadata"); - m_selectedTabEntry = m_rootMetaTable.GetEntry("Selected"); - UsageReporting.Report(ResourceType.Shuffleboard, 0); - } - - public void DisableActuatorWidgets() - { - ApplyToAllComplexWidgets(x => x.DisableIfActuator()); - } - - public void EnableActuatorWidgets() - { - ApplyToAllComplexWidgets(x => x.EnableIfActuator()); - } - - public ShuffleboardTab GetTab(string title) - { - if (!m_tabs.TryGetValue(title, out var tab)) - { - tab = new ShuffleboardTab(this, title); - m_tabs.Add(title, tab); - m_tabsChanged = true; - } - return tab; - } - - public void SelectTab(int index) - { - m_selectedTabEntry.ForceSetDouble(index); - } - - public void SelectTab(string title) - { - m_selectedTabEntry.ForceSetString(title); - } - - public void Update() - { - if (m_tabsChanged) - { - var tabTitles = m_tabs.Values.Select(x => x.Title).ToArray(); - m_rootMetaTable.GetEntry("Tabs").ForceSetStringArray(tabTitles); - m_tabsChanged = false; - } - - foreach (var tab in m_tabs.Values) - { - var title = tab.Title; - tab.BuildInto(m_rootTable, m_rootMetaTable.GetSubTable(title)); - } - } - - private void ApplyToAllComplexWidgets(Action func) - { - foreach (var tab in m_tabs.Values) - { - Apply(tab, func); - } - } - - private void Apply(IShuffleboardContainer container, Action func) - { - foreach (var component in container.Components) - { - if (component is ComplexWidget complexWidget) - { - func(complexWidget); - } - if (component is IShuffleboardContainer shuffleContainer) - { - Apply(shuffleContainer, func); - } - } - } - } -} diff --git a/src/wpilibsharp/ShuffleboardNS/ShuffleboardLayout.cs b/src/wpilibsharp/ShuffleboardNS/ShuffleboardLayout.cs deleted file mode 100644 index 8d5b9b55..00000000 --- a/src/wpilibsharp/ShuffleboardNS/ShuffleboardLayout.cs +++ /dev/null @@ -1,118 +0,0 @@ -using System; -using System.Collections.Generic; -using NetworkTables; -using WPILib.SmartDashboardNS; - -namespace WPILib.ShuffleboardNS -{ - public class ShuffleboardLayout : ShuffleboardComponent, IShuffleboardContainer - { - private readonly ContainerHelper m_helper; - - internal ShuffleboardLayout(IShuffleboardContainer parent, string? name, string type) - : base(parent, type, name) - { - m_helper = new ContainerHelper(this); - } - - public List Components => m_helper.Components; - - public ComplexWidget Add(string title, ISendable sendable) - { - return m_helper.Add(title, sendable); - } - - public ComplexWidget Add(ISendable sendable) - { - return m_helper.Add(sendable); - } - - public SimpleWidget Add(string title, T defaultValue) - { - return m_helper.Add(title, defaultValue); - } - - public SuppliedValueWidget Add(string title, Func valueSupplier) - { - return m_helper.Add(title, valueSupplier); - } - - public SuppliedValueWidget AddBoolean(string title, Func valueSupplier) - { - return m_helper.AddBoolean(title, valueSupplier); - } - - public SuppliedValueWidget AddBooleanArray(string title, Func valueSupplier) - { - return m_helper.AddBooleanArray(title, valueSupplier); - } - - public SuppliedValueWidget AddDoubleArray(string title, Func valueSupplier) - { - return m_helper.AddDoubleArray(title, valueSupplier); - } - - public SuppliedValueWidget AddNumber(string title, Func valueSupplier) - { - return m_helper.AddNumber(title, valueSupplier); - } - - public SimpleWidget AddPersistent(string title, T defaultValue) - { - var widget = Add(title, defaultValue); - widget.Entry.SetPersistent(); - return widget; - } - - public SuppliedValueWidget AddRaw(string title, Func valueSupplier) - { - return m_helper.AddRaw(title, valueSupplier); - } - - public SuppliedValueWidget AddString(string title, Func valueSupplier) - { - return m_helper.AddString(title, valueSupplier); - } - - public SuppliedValueWidget AddStringArray(string title, Func valueSupplier) - { - return m_helper.AddStringArray(title, valueSupplier); - } - - public override void BuildInto(NetworkTable parentTable, NetworkTable metaTable) - { - if (parentTable == null) - { - throw new ArgumentNullException(nameof(parentTable)); - } - - BuildMetadata(metaTable); - var table = parentTable.GetSubTable(Title); - table.GetEntry(".type").SetString("ShuffleboardLayout"); - foreach (var component in Components) - { - component.BuildInto(table, metaTable.GetSubTable(component.Title)); - } - } - - public ShuffleboardLayout GetLayout(string title, string type) - { - return m_helper.GetLayout(title, type); - } - - public ShuffleboardLayout GetLayout(string title, ILayoutType layoutType) - { - if (layoutType == null) - { - throw new ArgumentNullException(nameof(layoutType)); - } - - return m_helper.GetLayout(title, layoutType.LayoutName); - } - - public ShuffleboardLayout GetLayout(string title) - { - return m_helper.GetLayout(title); - } - } -} diff --git a/src/wpilibsharp/ShuffleboardNS/ShuffleboardTab.cs b/src/wpilibsharp/ShuffleboardNS/ShuffleboardTab.cs deleted file mode 100644 index 6774d03a..00000000 --- a/src/wpilibsharp/ShuffleboardNS/ShuffleboardTab.cs +++ /dev/null @@ -1,127 +0,0 @@ -using System; -using System.Collections.Generic; -using NetworkTables; -using WPILib.SmartDashboardNS; - -namespace WPILib.ShuffleboardNS -{ - public sealed class ShuffleboardTab : IShuffleboardContainer - { - private readonly ContainerHelper m_helper; - - internal ShuffleboardTab(IShuffleboardRoot root, string title) - { - m_helper = new ContainerHelper(this); - Root = root; - Title = title; - } - - public string Title { get; } - - public List Components => m_helper.Components; - - internal IShuffleboardRoot Root { get; } - - public void BuildInto(NetworkTable parentTable, NetworkTable metaTable) - { - if (parentTable == null) - { - throw new ArgumentNullException(nameof(parentTable)); - } - - if (metaTable == null) - { - throw new ArgumentNullException(nameof(metaTable)); - } - - var tabTable = parentTable.GetSubTable(Title); - tabTable.GetEntry(".type").SetString("ShuffleboardTab"); - foreach (var component in m_helper.Components) - { - component.BuildInto(tabTable, metaTable.GetSubTable(component.Title)); - } - } - - public ShuffleboardLayout GetLayout(string title, string type) - { - return m_helper.GetLayout(title, type); - } - - public ShuffleboardLayout GetLayout(string title, ILayoutType layoutType) - { - if (layoutType == null) - { - throw new ArgumentNullException(nameof(layoutType)); - } - - return m_helper.GetLayout(title, layoutType.LayoutName); - } - - public ShuffleboardLayout GetLayout(string title) - { - return m_helper.GetLayout(title); - } - - public ComplexWidget Add(string title, ISendable sendable) - { - return m_helper.Add(title, sendable); - } - - public ComplexWidget Add(ISendable sendable) - { - return m_helper.Add(sendable); - } - - public SimpleWidget Add(string title, T defaultValue) - { - return m_helper.Add(title, defaultValue); - } - - public SuppliedValueWidget Add(string title, Func valueSupplier) - { - return m_helper.Add(title, valueSupplier); - } - - public SuppliedValueWidget AddBoolean(string title, Func valueSupplier) - { - return m_helper.AddBoolean(title, valueSupplier); - } - - public SuppliedValueWidget AddBooleanArray(string title, Func valueSupplier) - { - return m_helper.AddBooleanArray(title, valueSupplier); - } - - public SuppliedValueWidget AddDoubleArray(string title, Func valueSupplier) - { - return m_helper.AddDoubleArray(title, valueSupplier); - } - - public SuppliedValueWidget AddNumber(string title, Func valueSupplier) - { - return m_helper.AddNumber(title, valueSupplier); - } - - public SimpleWidget AddPersistent(string title, T defaultValue) - { - var widget = Add(title, defaultValue); - widget.Entry.SetPersistent(); - return widget; - } - - public SuppliedValueWidget AddRaw(string title, Func valueSupplier) - { - return m_helper.AddRaw(title, valueSupplier); - } - - public SuppliedValueWidget AddString(string title, Func valueSupplier) - { - return m_helper.AddString(title, valueSupplier); - } - - public SuppliedValueWidget AddStringArray(string title, Func valueSupplier) - { - return m_helper.AddStringArray(title, valueSupplier); - } - } -} diff --git a/src/wpilibsharp/ShuffleboardNS/ShuffleboardWidget.cs b/src/wpilibsharp/ShuffleboardNS/ShuffleboardWidget.cs deleted file mode 100644 index bac5a76b..00000000 --- a/src/wpilibsharp/ShuffleboardNS/ShuffleboardWidget.cs +++ /dev/null @@ -1,31 +0,0 @@ -using System; - -namespace WPILib.ShuffleboardNS -{ - public abstract class ShuffleboardWidget - : ShuffleboardComponent where T : ShuffleboardWidget - { - - public ShuffleboardWidget(IShuffleboardContainer parent, string title) - : base(parent, title) - { - - } - - public T WithWidget(IWidgetType widgetType) - { - if (widgetType == null) - { - throw new ArgumentNullException(nameof(widgetType)); - } - - return WithWidget(widgetType.WidgetName); - } - - public T WithWidget(string widgetType) - { - Type = widgetType; - return (T)this; - } - } -} diff --git a/src/wpilibsharp/ShuffleboardNS/SimpleWidget.cs b/src/wpilibsharp/ShuffleboardNS/SimpleWidget.cs deleted file mode 100644 index 4d3d9c43..00000000 --- a/src/wpilibsharp/ShuffleboardNS/SimpleWidget.cs +++ /dev/null @@ -1,47 +0,0 @@ -using NetworkTables; - -namespace WPILib.ShuffleboardNS -{ - public sealed class SimpleWidget : ShuffleboardWidget - { - private NetworkTableEntry m_entry; - - internal SimpleWidget(IShuffleboardContainer parent, string title) - : base(parent, title) - { - - } - - public NetworkTableEntry Entry - { - get - { - if (!m_entry.IsValid) - { - ForceGenerate(); - } - return m_entry; - } - } - - public override void BuildInto(NetworkTable parentTable, NetworkTable metaTable) - { - BuildMetadata(metaTable); - if (!m_entry.IsValid) - { - m_entry = parentTable.GetEntry(Title); - } - } - - private void ForceGenerate() - { - var parent = Parent; - while (parent is ShuffleboardLayout layout) - { - parent = layout.Parent; - } - var tab = (ShuffleboardTab)parent; - tab.Root.Update(); - } - } -} diff --git a/src/wpilibsharp/ShuffleboardNS/SuppliedValueWidget.cs b/src/wpilibsharp/ShuffleboardNS/SuppliedValueWidget.cs deleted file mode 100644 index fcc3b23e..00000000 --- a/src/wpilibsharp/ShuffleboardNS/SuppliedValueWidget.cs +++ /dev/null @@ -1,29 +0,0 @@ -using System; -using NetworkTables; - -namespace WPILib.ShuffleboardNS -{ - public sealed class SuppliedValueWidget : ShuffleboardWidget> - { - private readonly Func m_supplier; - private readonly Action m_setter; - - internal SuppliedValueWidget(IShuffleboardContainer parent, string title, Func supplier, - Action setter) - : base(parent, title) - { - m_supplier = supplier; - m_setter = setter; - } - - public override void BuildInto(NetworkTable parentTable, NetworkTable metaTable) - { - - BuildMetadata(metaTable); - metaTable.GetEntry("Controllable").SetBoolean(false); - - var entry = parentTable.GetEntry(Title); - m_setter(entry, m_supplier()); - } - } -} diff --git a/src/wpilibsharp/SmartDashboardNS/CWTSendableDictionary.cs b/src/wpilibsharp/SmartDashboardNS/CWTSendableDictionary.cs deleted file mode 100644 index 667ac3d9..00000000 --- a/src/wpilibsharp/SmartDashboardNS/CWTSendableDictionary.cs +++ /dev/null @@ -1,45 +0,0 @@ -using System; -using System.Collections; -using System.Collections.Generic; -using System.Diagnostics.CodeAnalysis; -using System.Reflection; -using System.Runtime.CompilerServices; - -namespace WPILib.SmartDashboardNS -{ - internal class CWTSendableDictionary : ISendableDictionary - { - private readonly ConditionalWeakTable components = new ConditionalWeakTable(); - private readonly Func>> enumeratorGetter; - - public CWTSendableDictionary(MethodInfo getEnumeratorMethod) - { - enumeratorGetter = (Func>>)getEnumeratorMethod.CreateDelegate(typeof(Func>>), components); - } - - public void Add(ISendable key, SendableRegistry.Component value) - { - components.Add(key, value); - } - - public IEnumerator> GetEnumerator() - { - return enumeratorGetter(); - } - - public bool Remove(ISendable key) - { - return components.Remove(key); - } - - public bool TryGetValue(ISendable key, [MaybeNullWhen(false)]out SendableRegistry.Component value) - { - return components.TryGetValue(key, out value); - } - - IEnumerator IEnumerable.GetEnumerator() - { - return GetEnumerator(); - } - } -} diff --git a/src/wpilibsharp/SmartDashboardNS/ISendable.cs b/src/wpilibsharp/SmartDashboardNS/ISendable.cs deleted file mode 100644 index 377053d4..00000000 --- a/src/wpilibsharp/SmartDashboardNS/ISendable.cs +++ /dev/null @@ -1,7 +0,0 @@ -namespace WPILib.SmartDashboardNS -{ - public interface ISendable - { - void InitSendable(ISendableBuilder builder); - } -} diff --git a/src/wpilibsharp/SmartDashboardNS/ISendableBuilder.cs b/src/wpilibsharp/SmartDashboardNS/ISendableBuilder.cs deleted file mode 100644 index d8d6281c..00000000 --- a/src/wpilibsharp/SmartDashboardNS/ISendableBuilder.cs +++ /dev/null @@ -1,23 +0,0 @@ -using NetworkTables; -using System; - -namespace WPILib.SmartDashboardNS -{ - public interface ISendableBuilder - { - string SmartDashboardType { set; } - bool IsActuator { set; } - Action SafeState { set; } - Action UpdateTable { set; } - NetworkTableEntry GetEntry(string key); - - void AddBooleanProperty(string key, Func? getter, Action? setter); - void AddDoubleProperty(string key, Func? getter, Action? setter); - void AddStringProperty(string key, Func? getter, Action? setter); - void AddBooleanArrayProperty(string key, Func? getter, Action? setter); - void AddDoubleArrayProperty(string key, Func? getter, Action? setter); - void AddStringArrayProperty(string key, Func? getter, Action? setter); - void AddRawProperty(string key, Func? getter, Action? setter); - void AddValueProperty(string key, Func? getter, Action? setter); - } -} diff --git a/src/wpilibsharp/SmartDashboardNS/ISendableDictionary.cs b/src/wpilibsharp/SmartDashboardNS/ISendableDictionary.cs deleted file mode 100644 index 8ad1aaf5..00000000 --- a/src/wpilibsharp/SmartDashboardNS/ISendableDictionary.cs +++ /dev/null @@ -1,12 +0,0 @@ -using System.Collections.Generic; -using System.Diagnostics.CodeAnalysis; - -namespace WPILib.SmartDashboardNS -{ - internal interface ISendableDictionary : IEnumerable> - { - bool TryGetValue(ISendable key, [MaybeNullWhen(false)]out SendableRegistry.Component value); - void Add(ISendable key, SendableRegistry.Component value); - bool Remove(ISendable key); - } -} diff --git a/src/wpilibsharp/SmartDashboardNS/ListenerExecutor.cs b/src/wpilibsharp/SmartDashboardNS/ListenerExecutor.cs deleted file mode 100644 index 1b257833..00000000 --- a/src/wpilibsharp/SmartDashboardNS/ListenerExecutor.cs +++ /dev/null @@ -1,37 +0,0 @@ -using System; -using System.Collections.Generic; - -namespace WPILib.SmartDashboardNS -{ - internal class ListenerExecutor - { - private List m_tasks = new List(); - private List m_cacheTasks = new List(); - private readonly object m_lock = new object(); - - public void Execute(Action task) - { - lock (m_lock) - { - m_tasks.Add(task); - } - } - - public void RunListenerTasks() - { - List? tasks = null; - lock (m_lock) - { - tasks = m_tasks; - m_tasks = m_cacheTasks; - m_cacheTasks = tasks; - } - - foreach (var task in tasks) - { - task(); - } - tasks.Clear(); - } - } -} diff --git a/src/wpilibsharp/SmartDashboardNS/SendableBuilderImpl.cs b/src/wpilibsharp/SmartDashboardNS/SendableBuilderImpl.cs deleted file mode 100644 index e709fd57..00000000 --- a/src/wpilibsharp/SmartDashboardNS/SendableBuilderImpl.cs +++ /dev/null @@ -1,323 +0,0 @@ -using NetworkTables; -using NetworkTables.Natives; -using System; -using System.Collections.Generic; -using System.Diagnostics.CodeAnalysis; - -namespace WPILib.SmartDashboardNS -{ - public class SendableBuilderImpl : ISendableBuilder - { - private class Property : IDisposable - { - public NetworkTableEntry Entry { get; } - public NtEntryListener Listener { get; set; } - public Action? Update { get; set; } - public Func? CreateListener { get; set; } - - public Property(NetworkTable table, string key) - { - Entry = table.GetEntry(key); - Listener = new NtEntryListener(0); - Update = null; - CreateListener = null; - } - - public void StartListener() - { - if (Entry.IsValid && Listener.Get() == 0 && CreateListener != null) - { - Listener = CreateListener(Entry); - } - } - - public void StopListener() - { - if (Entry.IsValid && Listener.Get() != 0) - { - Entry.RemoveListener(Listener); - Listener = new NtEntryListener(0); - } - } - - public void Dispose() - { - StopListener(); - } - } - - private readonly List m_properties = new List(); - private readonly List m_updateTables = new List(); - private bool m_isActuator; - - private NetworkTableEntry m_controllableEntry; - - private NetworkTable? m_table; - - public string SmartDashboardType - { - set => m_table!.GetEntry(".type").SetString(value); - } - public bool IsActuator - { - get => m_isActuator; - set - { - m_table!.GetEntry(".actuator").SetBoolean(value); - m_isActuator = value; - } - } - [DisallowNull] - public Action? SafeState { private get; set; } - [DisallowNull] - public Action UpdateTable - { - set - { - m_updateTables.Add(value); - } - } - - - [DisallowNull] - internal NetworkTable? Table - { - get => m_table; - set - { - m_table = value; - m_controllableEntry = value!.GetEntry(".controllable"); - } - } - - public bool HasTable => m_table != null; - - public void TriggerUpdateTable() - { - foreach (var property in m_properties) - { - property.Update?.Invoke(property.Entry); - } - foreach (var updateTable in m_updateTables) - { - updateTable(); - } - } - - public void StartListeners() - { - foreach (var property in m_properties) - { - property.StartListener(); - } - if (m_controllableEntry.IsValid) - { - m_controllableEntry.SetBoolean(true); - } - } - - public void StopListeners() - { - foreach (var property in m_properties) - { - property.StopListener(); - } - if (m_controllableEntry.IsValid) - { - m_controllableEntry.SetBoolean(false); - } - } - - public void StartLiveWindowMode() - { - SafeState?.Invoke(); - StartListeners(); - } - - public void StopLiveWindowMode() - { - StopListeners(); - SafeState?.Invoke(); - } - - public void ClearProperties() - { - StopListeners(); - foreach (var property in m_properties) - { - property.Dispose(); - } - m_properties.Clear(); - } - - public void AddBooleanArrayProperty(string key, Func? getter, Action? setter) - { - var property = new Property(m_table!, key); - if (getter != null) - { - property.Update = entry => entry.SetBooleanArray(getter()); - } - if (setter != null) - { - property.CreateListener = entry => entry.AddListener((in RefEntryNotification evnt) => - { - if (evnt.Value.IsStringArray()) - { - bool[] v = evnt.Value.GetBooleanArray().ToArray(); - SmartDashboard.PostListenerTask(() => setter(v)); - } - }, NotifyFlags.Immediate | NotifyFlags.New | NotifyFlags.Update); - } - m_properties.Add(property); - } - - public void AddBooleanProperty(string key, Func? getter, Action? setter) - { - var property = new Property(m_table!, key); - if (getter != null) - { - property.Update = entry => entry.SetBoolean(getter()); - } - if (setter != null) - { - property.CreateListener = entry => entry.AddListener((in RefEntryNotification evnt) => - { - if (evnt.Value.IsBoolean()) - { - bool v = evnt.Value.GetBoolean(); - SmartDashboard.PostListenerTask(() => setter(v)); - } - }, NotifyFlags.Immediate | NotifyFlags.New | NotifyFlags.Update); - } - m_properties.Add(property); - } - - public void AddDoubleArrayProperty(string key, Func? getter, Action? setter) - { - var property = new Property(m_table!, key); - if (getter != null) - { - property.Update = entry => entry.SetDoubleArray(getter()); - } - if (setter != null) - { - property.CreateListener = entry => entry.AddListener((in RefEntryNotification evnt) => - { - if (evnt.Value.IsStringArray()) - { - double[] v = evnt.Value.GetDoubleArray().ToArray(); - SmartDashboard.PostListenerTask(() => setter(v)); - } - }, NotifyFlags.Immediate | NotifyFlags.New | NotifyFlags.Update); - } - m_properties.Add(property); - } - - public void AddDoubleProperty(string key, Func? getter, Action? setter) - { - var property = new Property(m_table!, key); - if (getter != null) - { - property.Update = entry => entry.SetDouble(getter()); - } - if (setter != null) - { - property.CreateListener = entry => entry.AddListener((in RefEntryNotification evnt) => - { - if (evnt.Value.IsDouble()) - { - double v = evnt.Value.GetDouble(); - SmartDashboard.PostListenerTask(() => setter(v)); - } - }, NotifyFlags.Immediate | NotifyFlags.New | NotifyFlags.Update); - } - m_properties.Add(property); - } - - public void AddRawProperty(string key, Func? getter, Action? setter) - { - var property = new Property(m_table!, key); - if (getter != null) - { - property.Update = entry => entry.SetRaw(getter()); - } - if (setter != null) - { - property.CreateListener = entry => entry.AddListener((in RefEntryNotification evnt) => - { - if (evnt.Value.IsStringArray()) - { - byte[] v = evnt.Value.GetRaw().ToArray(); - SmartDashboard.PostListenerTask(() => setter(v)); - } - }, NotifyFlags.Immediate | NotifyFlags.New | NotifyFlags.Update); - } - m_properties.Add(property); - } - - public void AddStringArrayProperty(string key, Func? getter, Action? setter) - { - var property = new Property(m_table!, key); - if (getter != null) - { - property.Update = entry => entry.SetStringArray(getter()); - } - if (setter != null) - { - property.CreateListener = entry => entry.AddListener((in RefEntryNotification evnt) => - { - if (evnt.Value.IsStringArray()) - { - string[] v = evnt.Value.GetStringArray().ToArray(); - SmartDashboard.PostListenerTask(() => setter(v)); - } - }, NotifyFlags.Immediate | NotifyFlags.New | NotifyFlags.Update); - } - m_properties.Add(property); - } - - public void AddStringProperty(string key, Func? getter, Action? setter) - { - var property = new Property(m_table!, key); - if (getter != null) - { - property.Update = entry => entry.SetString(getter()); - } - if (setter != null) - { - property.CreateListener = entry => entry.AddListener((in RefEntryNotification evnt) => - { - if (evnt.Value.IsString()) - { - string v = evnt.Value.GetString().ToString(); - SmartDashboard.PostListenerTask(() => setter(v)); - } - }, NotifyFlags.Immediate | NotifyFlags.New | NotifyFlags.Update); - } - m_properties.Add(property); - } - - public void AddValueProperty(string key, Func? getter, Action? setter) - { - var property = new Property(m_table!, key); - if (getter != null) - { - property.Update = entry => entry.SetValue(getter()); - } - if (setter != null) - { - property.CreateListener = entry => entry.AddListener((in RefEntryNotification evnt) => - { - var v = evnt.Value.ToValue(); - SmartDashboard.PostListenerTask(() => setter(v)); - }, NotifyFlags.Immediate | NotifyFlags.New | NotifyFlags.Update); - } - m_properties.Add(property); - } - - public NetworkTableEntry GetEntry(string key) - { - return m_table!.GetEntry(key); - } - } -} diff --git a/src/wpilibsharp/SmartDashboardNS/SendableChooser.cs b/src/wpilibsharp/SmartDashboardNS/SendableChooser.cs deleted file mode 100644 index 9c4fa53c..00000000 --- a/src/wpilibsharp/SmartDashboardNS/SendableChooser.cs +++ /dev/null @@ -1,103 +0,0 @@ -using System; -using System.Collections.Generic; -using System.Linq; -using System.Threading; -using NetworkTables; - -namespace WPILib.SmartDashboardNS -{ - public sealed class SendableChooser : ISendable, IDisposable - { - private const string DEFAULT = "default"; - private const string SELECTED = "selected"; - private const string ACTIVE = "active"; - private const string OPTIONS = "options"; - private const string INSTANCE = "instance"; - - private readonly Dictionary m_map = new Dictionary(); - private string m_defaultChoice = ""; - private readonly int m_instance; - private static int s_instances = 0; - - private string? m_selected; - private readonly List m_activeEntries = new List(); - private readonly object m_mutex = new object(); - - public SendableChooser() - { - m_instance = Interlocked.Increment(ref s_instances); - SendableRegistry.Instance.Add(this, "SendableChooser", m_instance); - } - - public void Dispose() - { - SendableRegistry.Instance.Remove(this); - } - - public void AddOption(string name, T option) - { - m_map.Add(name, option); - } - - public void SetDefaultOption(string name, T option) - { - m_defaultChoice = name; - AddOption(name, option); - } - - public T Selected - { - get - { - lock (m_mutex) - { - if (m_selected != null) - { - return m_map[m_selected]; - } - else - { - return m_map[m_defaultChoice]; - } - } - } - } - - void ISendable.InitSendable(ISendableBuilder builder) - { - builder.SmartDashboardType = "String Chooser"; - builder.GetEntry(INSTANCE).SetDouble(m_instance); - builder.AddStringProperty(DEFAULT, () => m_defaultChoice, null); - builder.AddStringArrayProperty(OPTIONS, () => m_map.Keys.ToArray(), null); - builder.AddStringProperty(ACTIVE, () => - { - lock (m_mutex) - { - if (m_selected != null) - { - return m_selected; - } - else - { - return m_defaultChoice; - } - } - }, null); - lock (m_mutex) - { - m_activeEntries.Add(builder.GetEntry(ACTIVE)); - } - builder.AddStringProperty(SELECTED, null, val => - { - lock (m_mutex) - { - m_selected = val; - foreach (var entry in m_activeEntries) - { - entry.SetString(val); - } - } - }); - } - } -} diff --git a/src/wpilibsharp/SmartDashboardNS/SendableRegistry.cs b/src/wpilibsharp/SmartDashboardNS/SendableRegistry.cs deleted file mode 100644 index d6e91caa..00000000 --- a/src/wpilibsharp/SmartDashboardNS/SendableRegistry.cs +++ /dev/null @@ -1,475 +0,0 @@ -using NetworkTables; -using System; -using System.Collections.Generic; -using System.Globalization; -using System.Linq; -using System.Reflection; -using System.Runtime.CompilerServices; - -[assembly: InternalsVisibleTo("wpilibsharp.test")] - -namespace WPILib.SmartDashboardNS -{ - public class SendableRegistry - { - private static readonly Lazy instance = new Lazy(() => new SendableRegistry()); - public static SendableRegistry Instance => instance.Value; - - internal class Component - { - public WeakReference? Sendable { get; set; } - public SendableBuilderImpl Builder { get; } = new SendableBuilderImpl(); - public string Name { get; set; } = ""; - public string Subsystem { get; set; } = "Ungrouped"; - public WeakReference? Parent { get; set; } - public bool LiveWindow { get; set; } - public object?[]? Data { get; set; } - - public Component() - { - } - - public Component(ISendable sendable) - { - Sendable = new WeakReference(sendable); - } - - public void SetName(string moduleType, int channel) - { - Name = $"{moduleType} [{channel.ToString(CultureInfo.InvariantCulture)}]"; - } - - public void SetName(string moduleType, int moduleNumber, int channel) - { - Name = $"{moduleType} [{moduleNumber.ToString(CultureInfo.InvariantCulture)},{channel.ToString(CultureInfo.InvariantCulture)}]"; - } - } - - private readonly object mutex = new object(); - - - private readonly ISendableDictionary components; - //private readonly ConditionalWeakTable components = new ConditionalWeakTable(); - //private readonly Dictionary components = new Dictionary(); - private int nextDataHandle = 0; - - private Component GetOrAdd(ISendable sendable) - { - if (!components.TryGetValue(sendable, out var comp)) - { - comp = new Component(sendable); - components.Add(sendable, comp); - } - else - { - if (comp.Sendable == null) - { - comp.Sendable = new WeakReference(sendable); - } - } - return comp; - } - - -#pragma warning disable CA1034 // Nested types should not be visible -#pragma warning disable CA1815 // Override equals and operator equals on value types - public struct CallbackData -#pragma warning restore CA1815 // Override equals and operator equals on value types -#pragma warning restore CA1034 // Nested types should not be visible - { - public ISendable Sendable { get; } - public string Name { get; } - public string Subsystem { get; } - public ISendable? Parent { get; } - public object? Data { get; set; } - public SendableBuilderImpl Builder { get; } - public CallbackData(ISendable sendable, string name, string subsystem, ISendable? parent, object? data, SendableBuilderImpl builder) - { - Sendable = sendable; - Name = name; - Subsystem = subsystem; - Parent = parent; - Data = data; - Builder = builder; - } - } - - internal SendableRegistry() - { - MethodInfo? cwtEnumerable = typeof(ConditionalWeakTable).GetMethods(BindingFlags.NonPublic | BindingFlags.Public | BindingFlags.Instance) - .Where(x => x.Name.EndsWith("GetEnumerator", StringComparison.CurrentCulture)) - .Where(x => x.ReturnType == typeof(IEnumerator>) && x.GetParameters().Length == 0) - .FirstOrDefault(); - - if (cwtEnumerable == null) - { - components = new StrongSendableDictionary(); - } - else - { - components = new CWTSendableDictionary(cwtEnumerable); - } - } - - public void Add(ISendable sendable, string name) - { - lock (mutex) - { - var comp = GetOrAdd(sendable); - comp.Name = name; - } - } - - public void Add(ISendable sendable, string moduleType, int channel) - { - lock (mutex) - { - GetOrAdd(sendable).SetName(moduleType, channel); - } - } - - public void Add(ISendable sendable, string moduleType, int moduleNumber, int channel) - { - lock (mutex) - { - GetOrAdd(sendable).SetName(moduleType, moduleNumber, channel); - } - } - - public void Add(ISendable sendable, string subsystem, string name) - { - lock (mutex) - { - var comp = GetOrAdd(sendable); - comp.Name = name; - comp.Subsystem = subsystem; - } - } - - public void AddLW(ISendable sendable, string name) - { - lock (mutex) - { - var comp = GetOrAdd(sendable); - comp.Name = name; - comp.LiveWindow = true; - } - } - - public void AddLW(ISendable sendable, string moduleType, int channel) - { - lock (mutex) - { - var comp = GetOrAdd(sendable); - comp.SetName(moduleType, channel); - comp.LiveWindow = true; - } - } - - public void AddLW(ISendable sendable, string moduleType, int moduleNumber, int channel) - { - lock (mutex) - { - var comp = GetOrAdd(sendable); - comp.SetName(moduleType, moduleNumber, channel); - comp.LiveWindow = true; - } - } - - public void AddLW(ISendable sendable, string subsystem, string name) - { - lock (mutex) - { - var comp = GetOrAdd(sendable); - comp.Name = name; - comp.Subsystem = subsystem; - comp.LiveWindow = true; - } - } - - public void AddChild(ISendable parent, ISendable child) - { - lock (mutex) - { - if (!components.TryGetValue(child, out var comp)) - { - comp = new Component(); - components.Add(child, comp); - } - comp.Parent = new WeakReference(parent); - } - } - - public bool Remove(ISendable sendable) - { - lock (mutex) - { - return components.Remove(sendable); - } - } - - public bool Contains(ISendable sendable) - { - lock (mutex) - { - return components.TryGetValue(sendable, out _); - } - } - - public string GetName(ISendable sendable) - { - lock (mutex) - { - if (components.TryGetValue(sendable, out var comp)) - { - return comp.Name; - } - return string.Empty; - } - } - - public void SetName(ISendable sendable, string name) - { - lock (mutex) - { - if (components.TryGetValue(sendable, out var comp)) - { - comp.Name = name; - } - } - } - - public void SetName(ISendable sendable, string moduleType, int channel) - { - lock (mutex) - { - if (components.TryGetValue(sendable, out var comp)) - { - comp.SetName(moduleType, channel); - } - } - } - - public void SetName(ISendable sendable, string moduleType, int moduleNumber, int channel) - { - lock (mutex) - { - if (components.TryGetValue(sendable, out var comp)) - { - comp.SetName(moduleType, moduleNumber, channel); - } - } - } - - public void SetName(ISendable sendable, string subsystem, string name) - { - lock (mutex) - { - if (components.TryGetValue(sendable, out var comp)) - { - comp.Name = name; - comp.Subsystem = subsystem; - } - } - } - - public string GetSubsystem(ISendable sendable) - { - lock (mutex) - { - if (components.TryGetValue(sendable, out var comp)) - { - return comp.Subsystem; - } - return string.Empty; - } - } - - public void SetSubsystem(ISendable sendable, string subsystem) - { - lock (mutex) - { - if (components.TryGetValue(sendable, out var comp)) - { - comp.Subsystem = subsystem; - } - } - } - - public int DataHandle - { - get - { - lock (mutex) - { - return nextDataHandle++; - } - } - } - - public object? SetData(ISendable sendable, int handle, object? data) - { - if (handle < 0) - { - throw new ArgumentOutOfRangeException(nameof(handle), "Handle must be 0 or greater"); - } - lock (mutex) - { - if (!components.TryGetValue(sendable, out var comp)) - { - return null; - } - object? rv = null; - if (comp.Data == null) - { - comp.Data = new object?[handle + 1]; - } - else if (handle < comp.Data.Length) - { - rv = comp.Data[handle]; - } - else - { - object?[] copy = new object?[handle + 1]; - comp.Data.CopyTo(copy.AsSpan()); - comp.Data = copy; - } - comp.Data[handle] = data; - return rv; - } - } - - public object? GetData(ISendable sendable, int handle) - { - if (handle < 0) - { - throw new ArgumentOutOfRangeException(nameof(handle), "Handle must be 0 or greater"); - } - lock (mutex) - { - if (!components.TryGetValue(sendable, out var comp)) - { - return null; - } - if (comp.Data == null || handle >= comp.Data.Length) - { - return null; - } - return comp.Data[handle]; - } - } - - public void EnableLiveWindow(ISendable sendable) - { - lock (mutex) - { - if (components.TryGetValue(sendable, out var comp)) - { - comp.LiveWindow = true; - } - } - } - - public void DisableLiveWindow(ISendable sendable) - { - lock (mutex) - { - if (components.TryGetValue(sendable, out var comp)) - { - comp.LiveWindow = false; - } - } - } - - public void Publish(ISendable sendable, NetworkTable table) - { - if (table == null) - { - throw new ArgumentNullException(nameof(table)); - } - - if (sendable == null) - { - throw new ArgumentNullException(nameof(sendable)); - } - - lock (mutex) - { - var comp = GetOrAdd(sendable); - comp.Builder.ClearProperties(); - comp.Builder.Table = table; - sendable.InitSendable(comp.Builder); - comp.Builder.TriggerUpdateTable(); - comp.Builder.StartListeners(); - } - } - - public void Update(ISendable sendable) - { - if (components.TryGetValue(sendable, out var comp)) - { - comp.Builder.TriggerUpdateTable(); - } - } - - public delegate void ForeachLiveWindowCallback(ref CallbackData data); - private readonly List foreachComponents = new List(); - - public void ForeachLiveWindow(int dataHandle, ForeachLiveWindowCallback callback) - { - lock (mutex) - { - foreachComponents.Clear(); - foreach (var v in components) - { - foreachComponents.Add(v.Value); - } - foreach (var comp in foreachComponents) - { - if (comp.Sendable == null) continue; - if (comp.Sendable == null || !comp.LiveWindow || !comp.Sendable.TryGetTarget(out var sendable)) - { - continue; - } - ISendable? parent = null; - if (comp.Parent != null) - { - comp.Parent.TryGetTarget(out parent); - } - object? data = null; - if (comp.Data != null && dataHandle < comp.Data.Length) - { - data = comp.Data[dataHandle]; - } - - CallbackData cbData = new CallbackData(sendable, comp.Name, comp.Subsystem, parent, data, comp.Builder); - - try - { - callback(ref cbData); - } -#pragma warning disable CA1031 // Do not catch general exception types - catch (Exception ex) -#pragma warning restore CA1031 // Do not catch general exception types - { - DriverStation.ReportError("Unhandled exception calling LiveWindow for " + comp.Name + ": " + ex.Message, false); - } - if (cbData.Data != null) - { - if (comp.Data == null) - { - comp.Data = new object?[dataHandle + 1]; - } - else if (dataHandle >= comp.Data.Length) - { - object?[] copy = new object?[dataHandle + 1]; - comp.Data.CopyTo(copy.AsSpan()); - comp.Data = copy; - } - comp.Data[dataHandle] = cbData.Data; - } - } - } - } - } -} diff --git a/src/wpilibsharp/SmartDashboardNS/SmartDashboard.cs b/src/wpilibsharp/SmartDashboardNS/SmartDashboard.cs deleted file mode 100644 index 9cabeb99..00000000 --- a/src/wpilibsharp/SmartDashboardNS/SmartDashboard.cs +++ /dev/null @@ -1,50 +0,0 @@ -using System; -using System.Collections.Generic; -using Hal; -using NetworkTables; - -namespace WPILib.SmartDashboardNS -{ - public static class SmartDashboard - { - private static readonly NetworkTable table = NetworkTableInstance.Default.GetTable("SmartDashboard"); - - static SmartDashboard() - { - UsageReporting.Report(ResourceType.SmartDashboard, 0); - } - - private static readonly ListenerExecutor listenerExecutor = new ListenerExecutor(); - - private static readonly Dictionary m_tablesToData = new Dictionary(); - - public static NetworkTableEntry GetEntry(string key) => table.GetEntry(key); - - public static bool ContainsKey(string key) => table.ContainsKey(key); - - public static HashSet GetKeys(NtType types) => table.GetKeys(types); - - public static HashSet GetKeys() => table.GetKeys(); - - public static void SetPersistent(string key) => GetEntry(key).SetPersistent(); - - public static bool IsPersistent(string key) => GetEntry(key).IsPersistent(); - - public static bool PutNumber(string key, double value) => GetEntry(key).SetDouble(value); - - public static void PostListenerTask(Action task) - { - listenerExecutor.Execute(task); - } - - public static void UpdateValues() - { - var inst = SendableRegistry.Instance; - foreach (var data in m_tablesToData.Values) - { - inst.Update(data); - } - listenerExecutor.RunListenerTasks(); - } - } -} diff --git a/src/wpilibsharp/SmartDashboardNS/StrongSendableDictionary.cs b/src/wpilibsharp/SmartDashboardNS/StrongSendableDictionary.cs deleted file mode 100644 index e6f60688..00000000 --- a/src/wpilibsharp/SmartDashboardNS/StrongSendableDictionary.cs +++ /dev/null @@ -1,36 +0,0 @@ -using System.Collections; -using System.Collections.Generic; -using System.Diagnostics.CodeAnalysis; - -namespace WPILib.SmartDashboardNS -{ - internal class StrongSendableDictionary : ISendableDictionary - { - private readonly Dictionary components = new Dictionary(); - - public void Add(ISendable key, SendableRegistry.Component value) - { - components.Add(key, value); - } - - public IEnumerator> GetEnumerator() - { - return components.GetEnumerator(); - } - - public bool Remove(ISendable key) - { - return components.Remove(key); - } - - public bool TryGetValue(ISendable key, [MaybeNullWhen(false)]out SendableRegistry.Component value) - { - return components.TryGetValue(key, out value); - } - - IEnumerator IEnumerable.GetEnumerator() - { - return GetEnumerator(); - } - } -} diff --git a/src/wpilibsharp/SynchronousInterrupt.cs b/src/wpilibsharp/SynchronousInterrupt.cs deleted file mode 100644 index 23b4aa55..00000000 --- a/src/wpilibsharp/SynchronousInterrupt.cs +++ /dev/null @@ -1,48 +0,0 @@ -using System; - -namespace WPILib -{ - public sealed class SynchronousInterrupt : IDisposable - { - private readonly IDigitalSource m_source; - - private readonly int m_interruptHandle; - - - public SynchronousInterrupt(IDigitalSource source) - { - m_source = source ?? throw new ArgumentNullException(nameof(source)); - m_interruptHandle = Hal.InterruptsLowLevel.Initialize(true); - Hal.InterruptsLowLevel.Request(m_interruptHandle, m_source.PortHandleForRouting, m_source.AnalogTriggerTypeForRouting); - } - - public EdgeConfiguration WaitForInterrupt(TimeSpan timeout, bool ignorePrevious = true) - { - var result = Hal.InterruptsLowLevel.WaitForInterrupt(m_interruptHandle, timeout.TotalSeconds, ignorePrevious); - - var rising = ((result & 0xFF) != 0) ? EdgeConfiguration.kRisingEdge : EdgeConfiguration.kNone; - var falling = ((result & 0xFF00) != 0) ? EdgeConfiguration.kFallingEdge : EdgeConfiguration.kNone; - return rising | falling; - } - - public void SetInterruptEdges(bool risingEdge, bool fallingEdge) - { - Hal.InterruptsLowLevel.SetInterruptUpSourceEdge(m_interruptHandle, risingEdge, fallingEdge); - } - - public TimeSpan RisingTimestamp => TimeSpan.FromTicks((long)(Hal.InterruptsLowLevel.ReadInterruptRisingTimestamp(m_interruptHandle) * Timer.TicksPerMicrosecond)); - - public TimeSpan FallingTimestamp => TimeSpan.FromTicks((long)(Hal.InterruptsLowLevel.ReadInterruptFallingTimestamp(m_interruptHandle) * Timer.TicksPerMicrosecond)); - - public void WakeupWaitingInterrupt() - { - Hal.InterruptsLowLevel.ReleaseWaitingInterrupt(m_interruptHandle); - } - - public unsafe void Dispose() - { - // Clean will wake up any waiting interrupts - Hal.InterruptsLowLevel.Clean(m_interruptHandle); - } - } -} diff --git a/src/wpilibsharp/TimedRobot.cs b/src/wpilibsharp/TimedRobot.cs deleted file mode 100644 index 131e8a41..00000000 --- a/src/wpilibsharp/TimedRobot.cs +++ /dev/null @@ -1,67 +0,0 @@ -using System; - -namespace WPILib -{ - public class TimedRobot : IterativeRobotBase - { - public static readonly TimeSpan DefaultPeriod = TimeSpan.FromMilliseconds(20); - - public override void StartCompetition() - { - RobotInit(); - - Hal.DriverStationLowLevel.ObserveUserProgramStarting(); - - m_expirationTime = Timer.FPGATimestamp + base.Period; - UpdateAlarm(); - - while (true) - { - ulong curTime = Hal.NotifierLowLevel.WaitForAlarm(m_notifier, out var status); - if (curTime == 0 || status != 0) break; - - m_expirationTime += base.Period; - - UpdateAlarm(); - - LoopFunc(); - } - } - - public override void EndCompetition() - { - Hal.NotifierLowLevel.Stop(m_notifier); - } - - public TimedRobot() : this(DefaultPeriod) - { - - } - - public TimedRobot(TimeSpan period) : base(period) - { - m_notifier = Hal.NotifierLowLevel.Initialize(); - Hal.NotifierLowLevel.SetName(m_notifier, "TimedRobot"); - - // Report - } - -#pragma warning disable CA1816 // Dispose methods should call SuppressFinalize - public override void Dispose() -#pragma warning restore CA1816 // Dispose methods should call SuppressFinalize - { - Hal.NotifierLowLevel.Stop(m_notifier); - Hal.NotifierLowLevel.Clean(m_notifier); - - base.Dispose(); - } - - private readonly int m_notifier; - private TimeSpan m_expirationTime; - - private void UpdateAlarm() - { - Hal.NotifierLowLevel.UpdateAlarm(m_notifier, (ulong)(m_expirationTime.TotalMilliseconds * 1000)); - } - } -} diff --git a/src/wpilibsharp/Timer.cs b/src/wpilibsharp/Timer.cs deleted file mode 100644 index fa3e2b9e..00000000 --- a/src/wpilibsharp/Timer.cs +++ /dev/null @@ -1,74 +0,0 @@ -using System; - -namespace WPILib -{ - public class Timer - { - public const long TicksPerMicrosecond = TimeSpan.TicksPerMillisecond / 1000; - - - public static TimeSpan FPGATimestamp => TimeSpan.FromTicks((long)(Hal.HALLowLevel.GetFPGATimestamp() * TicksPerMicrosecond)); - - private TimeSpan m_startTime; - private TimeSpan m_accumulatedTime; - private bool m_running; - private readonly object m_lockObject = new object(); - - public Timer() - { - Reset(); - } - - public TimeSpan Get() - { - lock (m_lockObject) - { - if (m_running) - { - return m_accumulatedTime + (FPGATimestamp - m_startTime); - } - else - { - return m_accumulatedTime; - } - } - } - - public void Reset() - { - lock (m_lockObject) - { - m_accumulatedTime = TimeSpan.Zero; - m_startTime = FPGATimestamp; - } - } - - public void Start() - { - lock (m_lockObject) - { - m_startTime = FPGATimestamp; - m_running = true; - } - } - - public void Stop() - { - lock (m_lockObject) - { - m_accumulatedTime = Get(); - m_running = false; - } - } - - public bool HasPeriodPassed(TimeSpan period) - { - if (Get() > period) - { - m_startTime += period; - return true; - } - return false; - } - } -} diff --git a/src/wpilibsharp/Watchdog.cs b/src/wpilibsharp/Watchdog.cs deleted file mode 100644 index ebf3763b..00000000 --- a/src/wpilibsharp/Watchdog.cs +++ /dev/null @@ -1,204 +0,0 @@ -using System; -using System.Collections.Generic; -using System.Threading; - -namespace WPILib -{ -#pragma warning disable CA1036 // Override methods on comparable types - public sealed class Watchdog : IDisposable, IComparable -#pragma warning restore CA1036 // Override methods on comparable types - { - private static readonly TimeSpan MinPrintPeriod = TimeSpan.FromSeconds(1); - - private TimeSpan m_startTime; - private TimeSpan m_timeout; - private TimeSpan m_expirationTime; - private readonly Action m_callback; - private TimeSpan m_lastTimeoutPrintTime; - private TimeSpan m_lastEpochsPrintTime; - - private readonly Dictionary m_epochs = new Dictionary(); - private bool m_isExpired; - - private static readonly Lazy m_lazyThread = new Lazy(() => - { - Thread inst = new Thread(SchedulerFunc); - inst.IsBackground = true; - inst.Name = "Watchdog Thread"; - inst.Start(); - return inst; - }, LazyThreadSafetyMode.ExecutionAndPublication); - - private static readonly LinkedList m_watchdogs = new LinkedList(); - private static readonly object m_lockObject = new object(); - - public Watchdog(TimeSpan timeout, Action callback) - { - m_timeout = timeout; - m_callback = callback; - var tmp = m_lazyThread.Value; - } - - public void Dispose() - { - Disable(); - } - - public int CompareTo(Watchdog? other) - { - if (other == null) - { - throw new ArgumentNullException(nameof(other)); - } - - return m_expirationTime.CompareTo(other.m_expirationTime); - } - - public TimeSpan Time => Timer.FPGATimestamp - m_startTime; - - public TimeSpan Timeout - { - get - { - lock (m_lockObject) - { - return m_timeout; - } - } - set - { - m_startTime = Timer.FPGATimestamp; - m_epochs.Clear(); - - lock (m_lockObject) - { - m_timeout = value; - m_isExpired = false; - m_watchdogs.Remove(this); - m_expirationTime = m_startTime + m_timeout; - m_watchdogs.AddLast(this); - Monitor.PulseAll(m_lockObject); - } - } - } - - public bool IsExpired - { - get - { - lock (m_lockObject) - { - return m_isExpired; - } - } - } - - public void AddEpoch(string epochName) - { - var currentTime = Timer.FPGATimestamp; - m_epochs.Add(epochName, currentTime - m_startTime); - m_startTime = currentTime; - } - - public void PrintEpochs() - { - var now = Timer.FPGATimestamp; - if (now - m_lastEpochsPrintTime > MinPrintPeriod) - { - m_lastEpochsPrintTime = now; - foreach (var epoch in m_epochs) - { - Console.WriteLine($"\t{epoch.Key}: {epoch.Value.ToString()}"); - } - } - } - - public void Reset() - { - Enable(); - } - - public void Enable() - { - m_startTime = Timer.FPGATimestamp; - - m_epochs.Clear(); - - lock (m_lockObject) - { - m_isExpired = false; - - m_watchdogs.Remove(this); - m_expirationTime = m_startTime + m_timeout; - m_watchdogs.AddLast(this); - Monitor.PulseAll(m_lockObject); - } - } - - public void Disable() - { - lock (m_lockObject) - { - m_watchdogs.Remove(this); - Monitor.PulseAll(m_lockObject); - } - } - - public bool SuppressTimeoutMessage { get; set; } - - private static void SchedulerFunc() - { - lock (m_lockObject) - { - while (true) - { - if (m_watchdogs.Count > 0) - { - bool timedOut = Monitor.Wait(m_lockObject, m_watchdogs.First!.Value.m_expirationTime - Timer.FPGATimestamp); - if (timedOut) - { - if (m_watchdogs.Count == 0 || m_watchdogs.First.Value.m_expirationTime > Timer.FPGATimestamp) - { - continue; - } - } - - var watchdogNode = m_watchdogs.First; - Watchdog watchdog = watchdogNode.Value; - m_watchdogs.Remove(watchdogNode); - - var now = Timer.FPGATimestamp; - if (now - watchdog.m_lastTimeoutPrintTime > MinPrintPeriod) - { - watchdog.m_lastTimeoutPrintTime = now; - if (!watchdog.SuppressTimeoutMessage) - { - Console.WriteLine($"Watchdog not fed within {watchdog.m_timeout}"); - } - } - - // Set expiration flag before calling the callback so any - // manipulation of the flag in the callback (e.g., calling - // Disable()) isn't clobbered. - watchdog.m_isExpired = true; - - Monitor.Exit(m_lockObject); - watchdog.m_callback?.Invoke(); - Monitor.Enter(m_lockObject); - - } - // Otherwise, a Watchdog removed itself from the queue (it notifies - // the scheduler of this) or a spurious wakeup occurred, so just - // rewait with the soonest watchdog timeout. - else - { - while (m_watchdogs.Count == 0) - { - Monitor.Wait(m_lockObject); - } - } - } - } - } - } -} diff --git a/src/wpilibsharp/wpilibsharp.csproj b/src/wpilibsharp/wpilibsharp.csproj index 6308ea15..81ce0538 100644 --- a/src/wpilibsharp/wpilibsharp.csproj +++ b/src/wpilibsharp/wpilibsharp.csproj @@ -1,36 +1,18 @@ - - - - WPILibSharp - WPILib - 1701;1702;1591;CA1303;CA1044;CA2225 - - - - - all - runtime; build; native; contentfiles; analyzers; buildtransitive - - - - - - - - - - - - - - - - - - - all - runtime; build; native; contentfiles; analyzers; buildtransitive - - - - + + + + WPILibSharp + WPILib + 1591;CA1401 + true + latest + true + + + + + + + + + diff --git a/test/wpiutil.test/WPIEventTest.cs b/test/wpiutil.test/WPIEventTest.cs new file mode 100644 index 00000000..e8bda5a7 --- /dev/null +++ b/test/wpiutil.test/WPIEventTest.cs @@ -0,0 +1,12 @@ +using WPIUtil.Concurrent; +using Xunit; + +namespace WPIUtil.Test; + +public class WPIEventTest { + [Fact] + public void TestEvent() { + using Event evnt = new(); + using Event evnt2 = new(); + } +}