diff --git a/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp b/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp index dd7c578c57..6b610eadab 100644 --- a/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp +++ b/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp @@ -49,6 +49,20 @@ DoubleSolenoid::DoubleSolenoid(std::shared_ptr module, m_reverseMask = 1 << reverseChannel; m_mask = m_forwardMask | m_reverseMask; + int allocMask = m_module->CheckAndReserveSolenoids(m_mask); + if (allocMask != 0) { + if (allocMask == m_mask) { + throw FRC_MakeError(err::ResourceAlreadyAllocated, "Channels {} and {}", + m_forwardChannel, m_reverseChannel); + } else if (allocMask == m_forwardMask) { + throw FRC_MakeError(err::ResourceAlreadyAllocated, "Channel {}", + m_forwardChannel); + } else { + throw FRC_MakeError(err::ResourceAlreadyAllocated, "Channel {}", + m_reverseChannel); + } + } + HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_forwardChannel + 1, m_module->GetModuleNumber() + 1); HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_reverseChannel + 1, @@ -58,7 +72,9 @@ DoubleSolenoid::DoubleSolenoid(std::shared_ptr module, m_module->GetModuleNumber(), m_forwardChannel); } -DoubleSolenoid::~DoubleSolenoid() {} +DoubleSolenoid::~DoubleSolenoid() { + m_module->UnreserveSolenoids(m_mask); +} void DoubleSolenoid::Set(Value value) { int setValue = 0; diff --git a/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp b/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp index 901a107bde..540a7154f7 100644 --- a/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp +++ b/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp @@ -6,6 +6,8 @@ #include #include +#include +#include #include "frc/Errors.h" #include "frc/SensorUtil.h" @@ -21,6 +23,7 @@ PneumaticsControlModule::PneumaticsControlModule(int module) { m_handle = HAL_InitializeCTREPCM(module, stackTrace.c_str(), &status); FRC_CheckErrorStatus(status, "Module {}", module); m_module = module; + wpi::SendableRegistry::AddLW(this, "Compressor", module); } PneumaticsControlModule::~PneumaticsControlModule() { @@ -160,3 +163,29 @@ void PneumaticsControlModule::SetOneShotDuration(int index, bool PneumaticsControlModule::CheckSolenoidChannel(int channel) const { return HAL_CheckCTREPCMSolenoidChannel(channel); } + +int PneumaticsControlModule::CheckAndReserveSolenoids(int mask) { + std::scoped_lock lock{m_reservedLock}; + uint32_t uMask = static_cast(mask); + if ((m_reservedMask & uMask) != 0) { + return m_reservedMask & uMask; + } + m_reservedMask |= uMask; + return 0; +} + +void PneumaticsControlModule::UnreserveSolenoids(int mask) { + std::scoped_lock lock{m_reservedLock}; + m_reservedMask &= ~(static_cast(mask)); +} + +void PneumaticsControlModule::InitSendable(wpi::SendableBuilder& builder) { + builder.SetSmartDashboardType("Compressor"); + builder.AddBooleanProperty( + "Closed Loop Control", [=]() { return GetClosedLoopControl(); }, + [=](bool value) { SetClosedLoopControl(value); }); + builder.AddBooleanProperty( + "Enabled", [=] { return GetCompressor(); }, nullptr); + builder.AddBooleanProperty( + "Pressure switch", [=]() { return GetPressureSwitch(); }, nullptr); +} diff --git a/wpilibc/src/main/native/cpp/Solenoid.cpp b/wpilibc/src/main/native/cpp/Solenoid.cpp index c8044c3be9..69723c0fbf 100644 --- a/wpilibc/src/main/native/cpp/Solenoid.cpp +++ b/wpilibc/src/main/native/cpp/Solenoid.cpp @@ -34,13 +34,19 @@ Solenoid::Solenoid(std::shared_ptr module, int channel) m_channel = channel; m_mask = 1 << channel; + if (m_module->CheckAndReserveSolenoids(m_mask) != 0) { + throw FRC_MakeError(err::ResourceAlreadyAllocated, "Channel {}", m_channel); + } + HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_channel + 1, m_module->GetModuleNumber() + 1); wpi::SendableRegistry::AddLW(this, "Solenoid", m_module->GetModuleNumber(), m_channel); } -Solenoid::~Solenoid() {} +Solenoid::~Solenoid() { + m_module->UnreserveSolenoids(m_mask); +} void Solenoid::Set(bool on) { int value = on ? (0xFFFF & m_mask) : 0; diff --git a/wpilibc/src/main/native/include/frc/PneumaticsBase.h b/wpilibc/src/main/native/include/frc/PneumaticsBase.h index fe7dcee88b..98e93594e0 100644 --- a/wpilibc/src/main/native/include/frc/PneumaticsBase.h +++ b/wpilibc/src/main/native/include/frc/PneumaticsBase.h @@ -24,5 +24,9 @@ class PneumaticsBase { virtual void SetOneShotDuration(int index, units::second_t duration) = 0; virtual bool CheckSolenoidChannel(int channel) const = 0; + + virtual int CheckAndReserveSolenoids(int mask) = 0; + + virtual void UnreserveSolenoids(int mask) = 0; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/PneumaticsControlModule.h b/wpilibc/src/main/native/include/frc/PneumaticsControlModule.h index 3366046763..6dc81e0331 100644 --- a/wpilibc/src/main/native/include/frc/PneumaticsControlModule.h +++ b/wpilibc/src/main/native/include/frc/PneumaticsControlModule.h @@ -5,20 +5,23 @@ #pragma once #include +#include +#include +#include #include "PneumaticsBase.h" namespace frc { -class PneumaticsControlModule : public PneumaticsBase { +class PneumaticsControlModule + : public PneumaticsBase, + public wpi::Sendable, + public wpi::SendableHelper { public: PneumaticsControlModule(); explicit PneumaticsControlModule(int module); ~PneumaticsControlModule() override; - PneumaticsControlModule(PneumaticsControlModule&&) = default; - PneumaticsControlModule& operator=(PneumaticsControlModule&&) = default; - bool GetCompressor(); void SetClosedLoopControl(bool enabled); @@ -55,8 +58,16 @@ class PneumaticsControlModule : public PneumaticsBase { bool CheckSolenoidChannel(int channel) const override; + int CheckAndReserveSolenoids(int mask) override; + + void UnreserveSolenoids(int mask) override; + + void InitSendable(wpi::SendableBuilder& builder) override; + private: int m_module; hal::Handle m_handle; + uint32_t m_reservedMask; + wpi::mutex m_reservedLock; }; } // namespace frc diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java index 958be43ca2..b06df929f6 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java @@ -6,6 +6,7 @@ package edu.wpi.first.wpilibj; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; +import edu.wpi.first.hal.util.AllocationException; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; @@ -42,7 +43,13 @@ public class DoubleSolenoid implements Sendable, AutoCloseable { public DoubleSolenoid(PneumaticsBase module, final int forwardChannel, final int reverseChannel) { m_module = Objects.requireNonNull(module, "Module cannot be null"); - // TODO check channels + if (!module.checkSolenoidChannel(forwardChannel)) { + throw new IllegalArgumentException("Channel " + forwardChannel + " out of range"); + } + + if (!module.checkSolenoidChannel(reverseChannel)) { + throw new IllegalArgumentException("Channel " + reverseChannel + " out of range"); + } m_forwardChannel = forwardChannel; m_reverseChannel = reverseChannel; @@ -51,6 +58,23 @@ public class DoubleSolenoid implements Sendable, AutoCloseable { m_reverseMask = 1 << reverseChannel; m_mask = m_forwardMask | m_reverseMask; + int allocMask = module.checkAndReserveSolenoids(m_mask); + if (allocMask != 0) { + if (allocMask == m_mask) { + throw new AllocationException( + "Channels " + forwardChannel + " and " + reverseChannel + " already allocated"); + } else if (allocMask == m_forwardMask) { + throw new AllocationException("Channel " + forwardChannel + " already allocated"); + } else { + throw new AllocationException("Channel " + reverseChannel + " already allocated"); + } + } + + if (module.checkAndReserveSolenoids(m_mask) != 0) { + + throw new AllocationException("Solenoid(s) already allocated"); + } + HAL.report( tResourceType.kResourceType_Solenoid, forwardChannel + 1, module.getModuleNumber() + 1); HAL.report( @@ -61,6 +85,7 @@ public class DoubleSolenoid implements Sendable, AutoCloseable { @Override public synchronized void close() { SendableRegistry.remove(this); + m_module.unreserveSolenoids(m_mask); m_module = null; } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsBase.java index 03f92ba66c..0d5d6d70e7 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsBase.java @@ -49,5 +49,26 @@ public interface PneumaticsBase extends AutoCloseable { */ void setOneShotDuration(int index, int durMs); + /** + * Check if a solenoid channel is valid. + * + * @param channel Channel to check + * @return True if channel exists + */ boolean checkSolenoidChannel(int channel); + + /** + * Check to see if the masked solenoids can be reserved, and if not reserve them. + * + * @param mask The solenoid mask to reserve + * @return 0 if successful, mask of solenoids that couldn't be allocated otherwise + */ + int checkAndReserveSolenoids(int mask); + + /** + * Unreserve the masked solenoids. + * + * @param mask The solenoid mask to unreserve + */ + void unreserveSolenoids(int mask); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsControlModule.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsControlModule.java index 62d025e731..678a51c831 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsControlModule.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsControlModule.java @@ -5,23 +5,36 @@ package edu.wpi.first.wpilibj; import edu.wpi.first.hal.CTREPCMJNI; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.util.sendable.SendableRegistry; -public class PneumaticsControlModule implements PneumaticsBase { +public class PneumaticsControlModule implements PneumaticsBase, Sendable { private final int m_handle; private final int m_module; + private int m_reservedMask; + private final Object m_reserveLock = new Object(); public PneumaticsControlModule() { this(SensorUtil.getDefaultCTREPCMModule()); } + /** + * Constructs a PneumaticsControlModule. + * + * @param module module number to construct + */ public PneumaticsControlModule(int module) { m_handle = CTREPCMJNI.initialize(module); m_module = module; + + SendableRegistry.addLW(this, "Compressor", module); } @Override public void close() throws Exception { CTREPCMJNI.free(m_handle); + SendableRegistry.remove(this); } public boolean getCompressor() { @@ -114,4 +127,31 @@ public class PneumaticsControlModule implements PneumaticsBase { public boolean checkSolenoidChannel(int channel) { return CTREPCMJNI.checkSolenoidChannel(channel); } + + @Override + public int checkAndReserveSolenoids(int mask) { + synchronized (m_reserveLock) { + if ((m_reservedMask & mask) != 0) { + return m_reservedMask & mask; + } + m_reservedMask |= mask; + return 0; + } + } + + @Override + public void unreserveSolenoids(int mask) { + synchronized (m_reserveLock) { + m_reservedMask &= ~mask; + } + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Compressor"); + builder.addBooleanProperty( + "Closed Loop Control", this::getClosedLoopControl, this::setClosedLoopControl); + builder.addBooleanProperty("Enabled", this::getCompressor, null); + builder.addBooleanProperty("Pressure switch", this::getPressureSwitch, null); + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java index d660f269c7..bfa064af10 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java @@ -6,6 +6,7 @@ package edu.wpi.first.wpilibj; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; +import edu.wpi.first.hal.util.AllocationException; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; @@ -38,6 +39,10 @@ public class Solenoid implements Sendable, AutoCloseable { m_mask = 1 << channel; m_channel = channel; + if (module.checkAndReserveSolenoids(m_mask) != 0) { + throw new AllocationException("Solenoid already allocated"); + } + HAL.report(tResourceType.kResourceType_Solenoid, channel + 1, module.getModuleNumber() + 1); SendableRegistry.addLW(this, "Solenoid", module.getModuleNumber(), channel); } @@ -45,6 +50,7 @@ public class Solenoid implements Sendable, AutoCloseable { @Override public void close() { SendableRegistry.remove(this); + m_module.unreserveSolenoids(m_mask); m_module = null; }