diff --git a/hal/src/main/native/sim/CTREPCM.cpp b/hal/src/main/native/sim/CTREPCM.cpp index a012b46be2..9d3119cf83 100644 --- a/hal/src/main/native/sim/CTREPCM.cpp +++ b/hal/src/main/native/sim/CTREPCM.cpp @@ -64,6 +64,12 @@ HAL_CTREPCMHandle HAL_InitializeCTREPCM(int32_t module, } void HAL_FreeCTREPCM(HAL_CTREPCMHandle handle) { + auto pcm = pcmHandles->Get(handle); + if (pcm == nullptr) { + pcmHandles->Free(handle); + return; + } + SimCTREPCMData[pcm->module].initialized = false; pcmHandles->Free(handle); } diff --git a/wpilibc/src/main/native/cpp/Compressor.cpp b/wpilibc/src/main/native/cpp/Compressor.cpp new file mode 100644 index 0000000000..0302acb3cb --- /dev/null +++ b/wpilibc/src/main/native/cpp/Compressor.cpp @@ -0,0 +1,72 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/Compressor.h" + +#include +#include +#include +#include + +#include "frc/Errors.h" + +using namespace frc; + +Compressor::Compressor(int module, PneumaticsModuleType moduleType) + : m_module{PneumaticsBase::GetForType(module, moduleType)} { + if (!m_module->ReserveCompressor()) { + throw FRC_MakeError(err::ResourceAlreadyAllocated, "{}", module); + } + + SetClosedLoopControl(true); + + HAL_Report(HALUsageReporting::kResourceType_Compressor, module + 1); + wpi::SendableRegistry::AddLW(this, "Compressor", module); +} + +Compressor::Compressor(PneumaticsModuleType moduleType) + : Compressor{PneumaticsBase::GetDefaultForType(moduleType), moduleType} {} + +Compressor::~Compressor() { + m_module->UnreserveCompressor(); +} + +void Compressor::Start() { + SetClosedLoopControl(true); +} + +void Compressor::Stop() { + SetClosedLoopControl(false); +} + +bool Compressor::Enabled() const { + return m_module->GetCompressor(); +} + +bool Compressor::GetPressureSwitchValue() const { + return m_module->GetPressureSwitch(); +} + +double Compressor::GetCompressorCurrent() const { + return m_module->GetCompressorCurrent(); +} + +void Compressor::SetClosedLoopControl(bool on) { + m_module->SetClosedLoopControl(on); +} + +bool Compressor::GetClosedLoopControl() const { + return m_module->GetClosedLoopControl(); +} + +void Compressor::InitSendable(wpi::SendableBuilder& builder) { + builder.SetSmartDashboardType("Compressor"); + builder.AddBooleanProperty( + "Closed Loop Control", [=]() { return GetClosedLoopControl(); }, + [=](bool value) { SetClosedLoopControl(value); }); + builder.AddBooleanProperty( + "Enabled", [=] { return Enabled(); }, nullptr); + builder.AddBooleanProperty( + "Pressure switch", [=]() { return GetPressureSwitchValue(); }, nullptr); +} diff --git a/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp b/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp index 6b610eadab..212673a988 100644 --- a/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp +++ b/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp @@ -18,33 +18,20 @@ using namespace frc; -DoubleSolenoid::DoubleSolenoid(PneumaticsBase& module, int forwardChannel, - int reverseChannel) - : DoubleSolenoid{std::shared_ptr{ - &module, wpi::NullDeleter()}, - forwardChannel, reverseChannel} {} - -DoubleSolenoid::DoubleSolenoid(PneumaticsBase* module, int forwardChannel, - int reverseChannel) - : DoubleSolenoid{std::shared_ptr{ - module, wpi::NullDeleter()}, - forwardChannel, reverseChannel} {} - -DoubleSolenoid::DoubleSolenoid(std::shared_ptr module, +DoubleSolenoid::DoubleSolenoid(int module, PneumaticsModuleType moduleType, int forwardChannel, int reverseChannel) - : m_module{std::move(module)} { - if (!m_module->CheckSolenoidChannel(forwardChannel)) { + : m_module{PneumaticsBase::GetForType(module, moduleType)}, + m_forwardChannel{forwardChannel}, + m_reverseChannel{reverseChannel} { + if (!m_module->CheckSolenoidChannel(m_forwardChannel)) { throw FRC_MakeError(err::ChannelIndexOutOfRange, "Channel {}", - forwardChannel); + m_forwardChannel); } - if (!m_module->CheckSolenoidChannel(reverseChannel)) { + if (!m_module->CheckSolenoidChannel(m_reverseChannel)) { throw FRC_MakeError(err::ChannelIndexOutOfRange, "Channel {}", - reverseChannel); + m_reverseChannel); } - m_forwardChannel = forwardChannel; - m_reverseChannel = reverseChannel; - m_forwardMask = 1 << forwardChannel; m_reverseMask = 1 << reverseChannel; m_mask = m_forwardMask | m_reverseMask; @@ -72,6 +59,11 @@ DoubleSolenoid::DoubleSolenoid(std::shared_ptr module, m_module->GetModuleNumber(), m_forwardChannel); } +DoubleSolenoid::DoubleSolenoid(PneumaticsModuleType moduleType, + int forwardChannel, int reverseChannel) + : DoubleSolenoid{PneumaticsBase::GetDefaultForType(moduleType), moduleType, + forwardChannel, reverseChannel} {} + DoubleSolenoid::~DoubleSolenoid() { m_module->UnreserveSolenoids(m_mask); } diff --git a/wpilibc/src/main/native/cpp/PneumaticsBase.cpp b/wpilibc/src/main/native/cpp/PneumaticsBase.cpp new file mode 100644 index 0000000000..ebe9775ee8 --- /dev/null +++ b/wpilibc/src/main/native/cpp/PneumaticsBase.cpp @@ -0,0 +1,26 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/PneumaticsBase.h" + +#include "frc/Errors.h" +#include "frc/PneumaticsControlModule.h" +#include "frc/SensorUtil.h" + +using namespace frc; + +std::shared_ptr PneumaticsBase::GetForType( + int module, PneumaticsModuleType moduleType) { + if (moduleType == PneumaticsModuleType::CTREPCM) { + return PneumaticsControlModule::GetForModule(module); + } + throw FRC_MakeError(err::InvalidParameter, "{}", moduleType); +} + +int PneumaticsBase::GetDefaultForType(PneumaticsModuleType moduleType) { + if (moduleType == PneumaticsModuleType::CTREPCM) { + return SensorUtil::GetDefaultCTREPCMModule(); + } + throw FRC_MakeError(err::InvalidParameter, "{}", moduleType); +} diff --git a/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp b/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp index 540a7154f7..32de5caf68 100644 --- a/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp +++ b/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp @@ -5,32 +5,79 @@ #include "frc/PneumaticsControlModule.h" #include +#include #include -#include -#include +#include "frc/Compressor.h" +#include "frc/DoubleSolenoid.h" #include "frc/Errors.h" #include "frc/SensorUtil.h" +#include "frc/Solenoid.h" using namespace frc; +wpi::mutex PneumaticsControlModule::m_handleLock; +std::unique_ptr< + wpi::DenseMap>> + PneumaticsControlModule::m_handleMap = nullptr; + +// Always called under lock, so we can avoid the double lock from the magic +// static +std::weak_ptr& +PneumaticsControlModule::GetDataStore(int module) { + if (!m_handleMap) { + m_handleMap = std::make_unique>>(); + } + return (*m_handleMap)[module]; +} + +class PneumaticsControlModule::DataStore { + public: + explicit DataStore(int module, const char* stackTrace) { + int32_t status = 0; + HAL_CTREPCMHandle handle = + HAL_InitializeCTREPCM(module, stackTrace, &status); + FRC_CheckErrorStatus(status, "Module {}", module); + m_moduleObject = PneumaticsControlModule{handle, module}; + m_moduleObject.m_dataStore = + std::shared_ptr{this, wpi::NullDeleter()}; + } + + ~DataStore() noexcept { HAL_FreeCTREPCM(m_moduleObject.m_handle); } + + DataStore(DataStore&&) = delete; + DataStore& operator=(DataStore&&) = delete; + + private: + friend class PneumaticsControlModule; + uint32_t m_reservedMask{0}; + bool m_compressorReserved{false}; + wpi::mutex m_reservedLock; + PneumaticsControlModule m_moduleObject{HAL_kInvalidHandle, 0}; +}; + PneumaticsControlModule::PneumaticsControlModule() : PneumaticsControlModule{SensorUtil::GetDefaultCTREPCMModule()} {} PneumaticsControlModule::PneumaticsControlModule(int module) { - int32_t status = 0; std::string stackTrace = wpi::GetStackTrace(1); - m_handle = HAL_InitializeCTREPCM(module, stackTrace.c_str(), &status); - FRC_CheckErrorStatus(status, "Module {}", module); + std::scoped_lock lock(m_handleLock); + auto& res = GetDataStore(module); + m_dataStore = res.lock(); + if (!m_dataStore) { + m_dataStore = std::make_shared(module, stackTrace.c_str()); + res = m_dataStore; + } + m_handle = m_dataStore->m_moduleObject.m_handle; m_module = module; - wpi::SendableRegistry::AddLW(this, "Compressor", module); } -PneumaticsControlModule::~PneumaticsControlModule() { - HAL_FreeCTREPCM(m_handle); -} +PneumaticsControlModule::PneumaticsControlModule(HAL_CTREPCMHandle handle, + int module) + : m_handle{handle}, m_module{module} {} -bool PneumaticsControlModule::GetCompressor() { +bool PneumaticsControlModule::GetCompressor() const { int32_t status = 0; auto result = HAL_GetCTREPCMCompressor(m_handle, &status); FRC_CheckErrorStatus(status, "Module {}", m_module); @@ -43,59 +90,59 @@ void PneumaticsControlModule::SetClosedLoopControl(bool enabled) { FRC_CheckErrorStatus(status, "Module {}", m_module); } -bool PneumaticsControlModule::GetClosedLoopControl() { +bool PneumaticsControlModule::GetClosedLoopControl() const { int32_t status = 0; auto result = HAL_GetCTREPCMClosedLoopControl(m_handle, &status); FRC_CheckErrorStatus(status, "Module {}", m_module); return result; } -bool PneumaticsControlModule::GetPressureSwitch() { +bool PneumaticsControlModule::GetPressureSwitch() const { int32_t status = 0; auto result = HAL_GetCTREPCMPressureSwitch(m_handle, &status); FRC_CheckErrorStatus(status, "Module {}", m_module); return result; } -double PneumaticsControlModule::GetCompressorCurrent() { +double PneumaticsControlModule::GetCompressorCurrent() const { int32_t status = 0; auto result = HAL_GetCTREPCMCompressorCurrent(m_handle, &status); FRC_CheckErrorStatus(status, "Module {}", m_module); return result; } -bool PneumaticsControlModule::GetCompressorCurrentTooHighFault() { +bool PneumaticsControlModule::GetCompressorCurrentTooHighFault() const { int32_t status = 0; auto result = HAL_GetCTREPCMCompressorCurrentTooHighFault(m_handle, &status); FRC_CheckErrorStatus(status, "Module {}", m_module); return result; } -bool PneumaticsControlModule::GetCompressorCurrentTooHighStickyFault() { +bool PneumaticsControlModule::GetCompressorCurrentTooHighStickyFault() const { int32_t status = 0; auto result = HAL_GetCTREPCMCompressorCurrentTooHighStickyFault(m_handle, &status); FRC_CheckErrorStatus(status, "Module {}", m_module); return result; } -bool PneumaticsControlModule::GetCompressorShortedFault() { +bool PneumaticsControlModule::GetCompressorShortedFault() const { int32_t status = 0; auto result = HAL_GetCTREPCMCompressorShortedFault(m_handle, &status); FRC_CheckErrorStatus(status, "Module {}", m_module); return result; } -bool PneumaticsControlModule::GetCompressorShortedStickyFault() { +bool PneumaticsControlModule::GetCompressorShortedStickyFault() const { int32_t status = 0; auto result = HAL_GetCTREPCMCompressorShortedStickyFault(m_handle, &status); FRC_CheckErrorStatus(status, "Module {}", m_module); return result; } -bool PneumaticsControlModule::GetCompressorNotConnectedFault() { +bool PneumaticsControlModule::GetCompressorNotConnectedFault() const { int32_t status = 0; auto result = HAL_GetCTREPCMCompressorNotConnectedFault(m_handle, &status); FRC_CheckErrorStatus(status, "Module {}", m_module); return result; } -bool PneumaticsControlModule::GetCompressorNotConnectedStickyFault() { +bool PneumaticsControlModule::GetCompressorNotConnectedStickyFault() const { int32_t status = 0; auto result = HAL_GetCTREPCMCompressorNotConnectedStickyFault(m_handle, &status); @@ -103,13 +150,13 @@ bool PneumaticsControlModule::GetCompressorNotConnectedStickyFault() { return result; } -bool PneumaticsControlModule::GetSolenoidVoltageFault() { +bool PneumaticsControlModule::GetSolenoidVoltageFault() const { int32_t status = 0; auto result = HAL_GetCTREPCMSolenoidVoltageFault(m_handle, &status); FRC_CheckErrorStatus(status, "Module {}", m_module); return result; } -bool PneumaticsControlModule::GetSolenoidVoltageStickyFault() { +bool PneumaticsControlModule::GetSolenoidVoltageStickyFault() const { int32_t status = 0; auto result = HAL_GetCTREPCMSolenoidVoltageStickyFault(m_handle, &status); FRC_CheckErrorStatus(status, "Module {}", m_module); @@ -165,27 +212,58 @@ bool PneumaticsControlModule::CheckSolenoidChannel(int channel) const { } int PneumaticsControlModule::CheckAndReserveSolenoids(int mask) { - std::scoped_lock lock{m_reservedLock}; + std::scoped_lock lock{m_dataStore->m_reservedLock}; uint32_t uMask = static_cast(mask); - if ((m_reservedMask & uMask) != 0) { - return m_reservedMask & uMask; + if ((m_dataStore->m_reservedMask & uMask) != 0) { + return m_dataStore->m_reservedMask & uMask; } - m_reservedMask |= uMask; + m_dataStore->m_reservedMask |= uMask; return 0; } void PneumaticsControlModule::UnreserveSolenoids(int mask) { - std::scoped_lock lock{m_reservedLock}; - m_reservedMask &= ~(static_cast(mask)); + std::scoped_lock lock{m_dataStore->m_reservedLock}; + m_dataStore->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); +bool PneumaticsControlModule::ReserveCompressor() { + std::scoped_lock lock{m_dataStore->m_reservedLock}; + if (m_dataStore->m_compressorReserved) { + return false; + } + m_dataStore->m_compressorReserved = true; + return true; +} + +void PneumaticsControlModule::UnreserveCompressor() { + std::scoped_lock lock{m_dataStore->m_reservedLock}; + m_dataStore->m_compressorReserved = false; +} + +Solenoid PneumaticsControlModule::MakeSolenoid(int channel) { + return Solenoid{m_module, PneumaticsModuleType::CTREPCM, channel}; +} + +DoubleSolenoid PneumaticsControlModule::MakeDoubleSolenoid(int forwardChannel, + int reverseChannel) { + return DoubleSolenoid{m_module, PneumaticsModuleType::CTREPCM, forwardChannel, + reverseChannel}; +} + +Compressor PneumaticsControlModule::MakeCompressor() { + return Compressor{m_module, PneumaticsModuleType::CTREPCM}; +} + +std::shared_ptr PneumaticsControlModule::GetForModule( + int module) { + std::string stackTrace = wpi::GetStackTrace(1); + std::scoped_lock lock(m_handleLock); + auto& res = GetDataStore(module); + std::shared_ptr dataStore = res.lock(); + if (!dataStore) { + dataStore = std::make_shared(module, stackTrace.c_str()); + res = dataStore; + } + + return std::shared_ptr{dataStore, &dataStore->m_moduleObject}; } diff --git a/wpilibc/src/main/native/cpp/Solenoid.cpp b/wpilibc/src/main/native/cpp/Solenoid.cpp index b3ff5cf167..49000c2bf6 100644 --- a/wpilibc/src/main/native/cpp/Solenoid.cpp +++ b/wpilibc/src/main/native/cpp/Solenoid.cpp @@ -16,22 +16,12 @@ using namespace frc; -Solenoid::Solenoid(PneumaticsBase& module, int channel) - : Solenoid{std::shared_ptr{ - &module, wpi::NullDeleter()}, - channel} {} - -Solenoid::Solenoid(PneumaticsBase* module, int channel) - : Solenoid{std::shared_ptr{ - module, wpi::NullDeleter()}, - channel} {} - -Solenoid::Solenoid(std::shared_ptr module, int channel) - : m_module{std::move(module)} { - if (!m_module->CheckSolenoidChannel(channel)) { - throw FRC_MakeError(err::ChannelIndexOutOfRange, "Channel {}", channel); +Solenoid::Solenoid(int module, PneumaticsModuleType moduleType, int channel) + : m_module{PneumaticsBase::GetForType(module, moduleType)}, + m_channel{channel} { + if (!m_module->CheckSolenoidChannel(m_channel)) { + throw FRC_MakeError(err::ChannelIndexOutOfRange, "Channel {}", m_channel); } - m_channel = channel; m_mask = 1 << channel; if (m_module->CheckAndReserveSolenoids(m_mask) != 0) { @@ -44,6 +34,10 @@ Solenoid::Solenoid(std::shared_ptr module, int channel) m_channel); } +Solenoid::Solenoid(PneumaticsModuleType moduleType, int channel) + : Solenoid{PneumaticsBase::GetDefaultForType(moduleType), moduleType, + channel} {} + Solenoid::~Solenoid() { m_module->UnreserveSolenoids(m_mask); } diff --git a/wpilibc/src/main/native/include/frc/Compressor.h b/wpilibc/src/main/native/include/frc/Compressor.h new file mode 100644 index 0000000000..cc065ffede --- /dev/null +++ b/wpilibc/src/main/native/include/frc/Compressor.h @@ -0,0 +1,116 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include +#include + +#include "frc/PneumaticsBase.h" +#include "frc/PneumaticsModuleType.h" +#include "frc/SensorUtil.h" + +namespace frc { + +/** + * Class for operating a compressor connected to a pneumatics module. + * + * The module will automatically run in closed loop mode by default whenever a + * Solenoid object is created. For most cases, a Compressor object does not need + * to be instantiated or used in a robot program. This class is only required in + * cases where the robot program needs a more detailed status of the compressor + * or to enable/disable closed loop control. + * + * Note: you cannot operate the compressor directly from this class as doing so + * would circumvent the safety provided by using the pressure switch and closed + * loop control. You can only turn off closed loop control, thereby stopping + * the compressor from operating. + */ +class Compressor : public wpi::Sendable, + public wpi::SendableHelper { + public: + /** + * Constructs a compressor for a specified module and type. + * + * @param module The module ID to use. + * @param moduleType The module type to use. + */ + Compressor(int module, PneumaticsModuleType moduleType); + + /** + * Constructs a compressor for a default module and specified type. + * + * @param moduleType The module type to use. + */ + explicit Compressor(PneumaticsModuleType moduleType); + + ~Compressor() override; + + Compressor(const Compressor&) = delete; + Compressor& operator=(const Compressor&) = delete; + + Compressor(Compressor&&) = default; + Compressor& operator=(Compressor&&) = default; + + /** + * Starts closed-loop control. Note that closed loop control is enabled by + * default. + */ + void Start(); + + /** + * Stops closed-loop control. Note that closed loop control is enabled by + * default. + */ + void Stop(); + + /** + * Check if compressor output is active. + * + * @return true if the compressor is on + */ + bool Enabled() const; + + /** + * Check if the pressure switch is triggered. + * + * @return true if pressure is low + */ + bool GetPressureSwitchValue() const; + + /** + * Query how much current the compressor is drawing. + * + * @return The current through the compressor, in amps + */ + double GetCompressorCurrent() const; + + /** + * Enables or disables automatically turning the compressor on when the + * pressure is low. + * + * @param on Set to true to enable closed loop control of the compressor. + * False to disable. + */ + void SetClosedLoopControl(bool on); + + /** + * Returns true if the compressor will automatically turn on when the + * pressure is low. + * + * @return True if closed loop control of the compressor is enabled. False if + * disabled. + */ + bool GetClosedLoopControl() const; + + void InitSendable(wpi::SendableBuilder& builder) override; + + private: + std::shared_ptr m_module; +}; + +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/DoubleSolenoid.h b/wpilibc/src/main/native/include/frc/DoubleSolenoid.h index 62957d7f13..381e3a1464 100644 --- a/wpilibc/src/main/native/include/frc/DoubleSolenoid.h +++ b/wpilibc/src/main/native/include/frc/DoubleSolenoid.h @@ -11,12 +11,13 @@ #include #include "frc/PneumaticsBase.h" +#include "frc/PneumaticsModuleType.h" namespace frc { /** * DoubleSolenoid class for running 2 channels of high voltage Digital Output - * (PCM). + * on a pneumatics module. * * The DoubleSolenoid class is typically used for pneumatics solenoids that * have two positions controlled by two separate channels. @@ -26,11 +27,27 @@ class DoubleSolenoid : public wpi::Sendable, public: enum Value { kOff, kForward, kReverse }; - DoubleSolenoid(PneumaticsBase& module, int forwardChannel, - int reverseChannel); - DoubleSolenoid(PneumaticsBase* module, int forwardChannel, - int reverseChannel); - DoubleSolenoid(std::shared_ptr module, int forwardChannel, + /** + * Constructs a double solenoid for a specified module of a specific module + * type. + * + * @param module The module of the solenoid module to use. + * @param moduleType The module type to use. + * @param forwardChannel The forward channel on the module to control. + * @param reverseChannel The reverse channel on the module to control. + */ + DoubleSolenoid(int module, PneumaticsModuleType moduleType, + int forwardChannel, int reverseChannel); + + /** + * Constructs a double solenoid for a default module of a specific module + * type. + * + * @param moduleType The module type to use. + * @param forwardChannel The forward channel on the module to control. + * @param reverseChannel The reverse channel on the module to control. + */ + DoubleSolenoid(PneumaticsModuleType moduleType, int forwardChannel, int reverseChannel); ~DoubleSolenoid() override; @@ -100,12 +117,12 @@ class DoubleSolenoid : public wpi::Sendable, void InitSendable(wpi::SendableBuilder& builder) override; private: + std::shared_ptr m_module; int m_forwardChannel; // The forward channel on the module to control. int m_reverseChannel; // The reverse channel on the module to control. int m_forwardMask; // The mask for the forward channel. int m_reverseMask; // The mask for the reverse channel. int m_mask; - std::shared_ptr m_module; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/PneumaticsBase.h b/wpilibc/src/main/native/include/frc/PneumaticsBase.h index 98e93594e0..06cad5dd97 100644 --- a/wpilibc/src/main/native/include/frc/PneumaticsBase.h +++ b/wpilibc/src/main/native/include/frc/PneumaticsBase.h @@ -4,13 +4,30 @@ #pragma once +#include + #include +#include "frc/PneumaticsModuleType.h" + namespace frc { +class Solenoid; +class DoubleSolenoid; +class Compressor; class PneumaticsBase { public: virtual ~PneumaticsBase() = default; + virtual bool GetCompressor() const = 0; + + virtual bool GetPressureSwitch() const = 0; + + virtual double GetCompressorCurrent() const = 0; + + virtual void SetClosedLoopControl(bool on) = 0; + + virtual bool GetClosedLoopControl() const = 0; + virtual void SetSolenoids(int mask, int values) = 0; virtual int GetSolenoids() const = 0; @@ -28,5 +45,18 @@ class PneumaticsBase { virtual int CheckAndReserveSolenoids(int mask) = 0; virtual void UnreserveSolenoids(int mask) = 0; + + virtual bool ReserveCompressor() = 0; + + virtual void UnreserveCompressor() = 0; + + virtual Solenoid MakeSolenoid(int channel) = 0; + virtual DoubleSolenoid MakeDoubleSolenoid(int forwardChannel, + int reverseChannel) = 0; + virtual Compressor MakeCompressor() = 0; + + static std::shared_ptr GetForType( + int module, PneumaticsModuleType moduleType); + static int GetDefaultForType(PneumaticsModuleType moduleType); }; } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/PneumaticsControlModule.h b/wpilibc/src/main/native/include/frc/PneumaticsControlModule.h index 46953d4250..64686d6085 100644 --- a/wpilibc/src/main/native/include/frc/PneumaticsControlModule.h +++ b/wpilibc/src/main/native/include/frc/PneumaticsControlModule.h @@ -4,43 +4,41 @@ #pragma once +#include + #include +#include #include -#include -#include #include "PneumaticsBase.h" namespace frc { -class PneumaticsControlModule - : public PneumaticsBase, - public wpi::Sendable, - public wpi::SendableHelper { +class PneumaticsControlModule : public PneumaticsBase { public: PneumaticsControlModule(); explicit PneumaticsControlModule(int module); - ~PneumaticsControlModule() override; + ~PneumaticsControlModule() override = default; - bool GetCompressor(); + bool GetCompressor() const override; - void SetClosedLoopControl(bool enabled); + void SetClosedLoopControl(bool enabled) override; - bool GetClosedLoopControl(); + bool GetClosedLoopControl() const override; - bool GetPressureSwitch(); + bool GetPressureSwitch() const override; - double GetCompressorCurrent(); + double GetCompressorCurrent() const override; - bool GetCompressorCurrentTooHighFault(); - bool GetCompressorCurrentTooHighStickyFault(); - bool GetCompressorShortedFault(); - bool GetCompressorShortedStickyFault(); - bool GetCompressorNotConnectedFault(); - bool GetCompressorNotConnectedStickyFault(); + bool GetCompressorCurrentTooHighFault() const; + bool GetCompressorCurrentTooHighStickyFault() const; + bool GetCompressorShortedFault() const; + bool GetCompressorShortedStickyFault() const; + bool GetCompressorNotConnectedFault() const; + bool GetCompressorNotConnectedStickyFault() const; - bool GetSolenoidVoltageFault(); - bool GetSolenoidVoltageStickyFault(); + bool GetSolenoidVoltageFault() const; + bool GetSolenoidVoltageStickyFault() const; void ClearAllStickyFaults(); @@ -62,12 +60,30 @@ class PneumaticsControlModule void UnreserveSolenoids(int mask) override; - void InitSendable(wpi::SendableBuilder& builder) override; + bool ReserveCompressor() override; + + void UnreserveCompressor() override; + + Solenoid MakeSolenoid(int channel) override; + DoubleSolenoid MakeDoubleSolenoid(int forwardChannel, + int reverseChannel) override; + Compressor MakeCompressor() override; private: + class DataStore; + friend class DataStore; + friend class PneumaticsBase; + PneumaticsControlModule(HAL_CTREPCMHandle handle, int module); + + static std::shared_ptr GetForModule(int module); + + std::shared_ptr m_dataStore; + HAL_CTREPCMHandle m_handle; int m_module; - hal::Handle m_handle; - uint32_t m_reservedMask{0}; - wpi::mutex m_reservedLock; + + static wpi::mutex m_handleLock; + static std::unique_ptr>> + m_handleMap; + static std::weak_ptr& GetDataStore(int module); }; } // namespace frc diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/PneumaticsModule.h b/wpilibc/src/main/native/include/frc/PneumaticsModuleType.h similarity index 63% rename from wpilibcExamples/src/main/cpp/examples/PacGoat/include/PneumaticsModule.h rename to wpilibc/src/main/native/include/frc/PneumaticsModuleType.h index cb9e5999ce..7f706620b5 100644 --- a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/PneumaticsModule.h +++ b/wpilibc/src/main/native/include/frc/PneumaticsModuleType.h @@ -4,8 +4,6 @@ #pragma once -#include - -namespace pac { -frc::PneumaticsControlModule* GetPneumatics(); -} // namespace pac +namespace frc { +enum class PneumaticsModuleType { CTREPCM, REVPH }; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/Solenoid.h b/wpilibc/src/main/native/include/frc/Solenoid.h index a59079ccd3..92bd69a254 100644 --- a/wpilibc/src/main/native/include/frc/Solenoid.h +++ b/wpilibc/src/main/native/include/frc/Solenoid.h @@ -12,20 +12,35 @@ #include #include "frc/PneumaticsBase.h" +#include "frc/PneumaticsModuleType.h" namespace frc { /** - * Solenoid class for running high voltage Digital Output (PCM). + * Solenoid class for running high voltage Digital Output on a pneumatics + * module. * * The Solenoid class is typically used for pneumatics solenoids, but could be - * used for any device within the current spec of the PCM. + * used for any device within the current spec of the module. */ class Solenoid : public wpi::Sendable, public wpi::SendableHelper { public: - Solenoid(PneumaticsBase& module, int channel); - Solenoid(PneumaticsBase* module, int channel); - Solenoid(std::shared_ptr module, int channel); + /** + * Constructs a solenoid for a specified module and type. + * + * @param module The module ID to use. + * @param moduleType The module type to use. + * @param channel The channel the solenoid is on. + */ + Solenoid(int module, PneumaticsModuleType moduleType, int channel); + + /** + * Constructs a solenoid for a default module and specified type. + * + * @param moduleType The module type to use. + * @param channel The channel the solenoid is on. + */ + Solenoid(PneumaticsModuleType moduleType, int channel); ~Solenoid() override; diff --git a/wpilibc/src/test/native/cpp/DoubleSolenoidTest.cpp b/wpilibc/src/test/native/cpp/DoubleSolenoidTest.cpp index e12c358ffe..9ed21d32e7 100644 --- a/wpilibc/src/test/native/cpp/DoubleSolenoidTest.cpp +++ b/wpilibc/src/test/native/cpp/DoubleSolenoidTest.cpp @@ -12,8 +12,7 @@ namespace frc { TEST(DoubleSolenoidTest, ValidInitialization) { - PneumaticsControlModule pcm{3}; - DoubleSolenoid solenoid{pcm, 2, 3}; + DoubleSolenoid solenoid{3, frc::PneumaticsModuleType::CTREPCM, 2, 3}; solenoid.Set(DoubleSolenoid::kReverse); EXPECT_EQ(DoubleSolenoid::kReverse, solenoid.Get()); @@ -25,30 +24,30 @@ TEST(DoubleSolenoidTest, ValidInitialization) { } TEST(DoubleSolenoidTest, ThrowForwardPortAlreadyInitialized) { - PneumaticsControlModule pcm{5}; // Single solenoid that is reused for forward port - Solenoid solenoid{pcm, 2}; - EXPECT_THROW(DoubleSolenoid(pcm, 2, 3), std::runtime_error); + Solenoid solenoid{5, frc::PneumaticsModuleType::CTREPCM, 2}; + EXPECT_THROW(DoubleSolenoid(5, frc::PneumaticsModuleType::CTREPCM, 2, 3), + std::runtime_error); } TEST(DoubleSolenoidTest, ThrowReversePortAlreadyInitialized) { - PneumaticsControlModule pcm{6}; // Single solenoid that is reused for forward port - Solenoid solenoid{pcm, 3}; - EXPECT_THROW(DoubleSolenoid(pcm, 2, 3), std::runtime_error); + Solenoid solenoid{6, frc::PneumaticsModuleType::CTREPCM, 3}; + EXPECT_THROW(DoubleSolenoid(6, frc::PneumaticsModuleType::CTREPCM, 2, 3), + std::runtime_error); } TEST(DoubleSolenoidTest, ThrowBothPortsAlreadyInitialized) { PneumaticsControlModule pcm{6}; // Single solenoid that is reused for forward port - Solenoid solenoid0(pcm, 2); - Solenoid solenoid1(pcm, 3); - EXPECT_THROW(DoubleSolenoid(pcm, 2, 3), std::runtime_error); + Solenoid solenoid0(6, frc::PneumaticsModuleType::CTREPCM, 2); + Solenoid solenoid1(6, frc::PneumaticsModuleType::CTREPCM, 3); + EXPECT_THROW(DoubleSolenoid(6, frc::PneumaticsModuleType::CTREPCM, 2, 3), + std::runtime_error); } TEST(DoubleSolenoidTest, Toggle) { - PneumaticsControlModule pcm{4}; - DoubleSolenoid solenoid{&pcm, 2, 3}; + DoubleSolenoid solenoid{4, frc::PneumaticsModuleType::CTREPCM, 2, 3}; // Bootstrap it into reverse solenoid.Set(DoubleSolenoid::kReverse); @@ -65,12 +64,12 @@ TEST(DoubleSolenoidTest, Toggle) { } TEST(DoubleSolenoidTest, InvalidForwardPort) { - PneumaticsControlModule pcm{0}; - EXPECT_THROW(DoubleSolenoid(pcm, 100, 1), std::runtime_error); + EXPECT_THROW(DoubleSolenoid(0, frc::PneumaticsModuleType::CTREPCM, 100, 1), + std::runtime_error); } TEST(DoubleSolenoidTest, InvalidReversePort) { - PneumaticsControlModule pcm{0}; - EXPECT_THROW(DoubleSolenoid(pcm, 0, 100), std::runtime_error); + EXPECT_THROW(DoubleSolenoid(0, frc::PneumaticsModuleType::CTREPCM, 0, 100), + std::runtime_error); } } // namespace frc diff --git a/wpilibc/src/test/native/cpp/SolenoidTest.cpp b/wpilibc/src/test/native/cpp/SolenoidTest.cpp index 1935d2fdc2..86e5b72a97 100644 --- a/wpilibc/src/test/native/cpp/SolenoidTest.cpp +++ b/wpilibc/src/test/native/cpp/SolenoidTest.cpp @@ -11,8 +11,7 @@ namespace frc { TEST(SolenoidTest, ValidInitialization) { - PneumaticsControlModule pcm{3}; - Solenoid solenoid{pcm, 2}; + Solenoid solenoid{3, frc::PneumaticsModuleType::CTREPCM, 2}; EXPECT_EQ(2, solenoid.GetChannel()); solenoid.Set(true); @@ -23,25 +22,24 @@ TEST(SolenoidTest, ValidInitialization) { } TEST(SolenoidTest, DoubleInitialization) { - PneumaticsControlModule pcm{3}; - Solenoid solenoid{&pcm, 2}; - EXPECT_THROW(Solenoid(pcm, 2), std::runtime_error); + Solenoid solenoid{3, frc::PneumaticsModuleType::CTREPCM, 2}; + EXPECT_THROW(Solenoid(3, frc::PneumaticsModuleType::CTREPCM, 2), + std::runtime_error); } TEST(SolenoidTest, DoubleInitializationFromDoubleSolenoid) { - PneumaticsControlModule pcm{3}; - DoubleSolenoid solenoid{pcm, 2, 3}; - EXPECT_THROW(Solenoid(pcm, 2), std::runtime_error); + DoubleSolenoid solenoid{3, frc::PneumaticsModuleType::CTREPCM, 2, 3}; + EXPECT_THROW(Solenoid(3, frc::PneumaticsModuleType::CTREPCM, 2), + std::runtime_error); } TEST(SolenoidTest, InvalidChannel) { - PneumaticsControlModule pcm{3}; - EXPECT_THROW(Solenoid(pcm, 100), std::runtime_error); + EXPECT_THROW(Solenoid(3, frc::PneumaticsModuleType::CTREPCM, 100), + std::runtime_error); } TEST(SolenoidTest, Toggle) { - PneumaticsControlModule pcm{3}; - Solenoid solenoid{pcm, 2}; + Solenoid solenoid{3, frc::PneumaticsModuleType::CTREPCM, 2}; solenoid.Set(true); EXPECT_TRUE(solenoid.Get()); diff --git a/wpilibc/src/test/native/cpp/simulation/CTREPCMSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/CTREPCMSimTest.cpp index 0af7572e75..a8f95da09e 100644 --- a/wpilibc/src/test/native/cpp/simulation/CTREPCMSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/CTREPCMSimTest.cpp @@ -33,7 +33,7 @@ TEST(CTREPCMSimTest, SolenoidOutput) { CTREPCMSim sim(pcm); sim.ResetData(); - DoubleSolenoid doubleSolenoid{pcm, 3, 4}; + DoubleSolenoid doubleSolenoid{0, frc::PneumaticsModuleType::CTREPCM, 3, 4}; BooleanCallback callback3; BooleanCallback callback4; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/HatchSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/HatchSubsystem.cpp index 26b61d0276..ba1c0dd58a 100644 --- a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/HatchSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/HatchSubsystem.cpp @@ -7,8 +7,8 @@ using namespace HatchConstants; HatchSubsystem::HatchSubsystem() - : m_hatchSolenoid{m_pneumaticsModule, kHatchSolenoidPorts[0], - kHatchSolenoidPorts[1]} {} + : m_hatchSolenoid{frc::PneumaticsModuleType::CTREPCM, + kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]} {} void HatchSubsystem::GrabHatch() { m_hatchSolenoid.Set(frc::DoubleSolenoid::kForward); diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/HatchSubsystem.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/HatchSubsystem.h index 0b6d256dfb..bb0610041e 100644 --- a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/HatchSubsystem.h +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/HatchSubsystem.h @@ -29,6 +29,5 @@ class HatchSubsystem : public frc2::SubsystemBase { private: // Components (e.g. motor controllers and sensors) should generally be // declared private and exposed only through public methods. - frc::PneumaticsControlModule m_pneumaticsModule; frc::DoubleSolenoid m_hatchSolenoid; }; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/HatchSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/HatchSubsystem.cpp index 26b61d0276..ba1c0dd58a 100644 --- a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/HatchSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/HatchSubsystem.cpp @@ -7,8 +7,8 @@ using namespace HatchConstants; HatchSubsystem::HatchSubsystem() - : m_hatchSolenoid{m_pneumaticsModule, kHatchSolenoidPorts[0], - kHatchSolenoidPorts[1]} {} + : m_hatchSolenoid{frc::PneumaticsModuleType::CTREPCM, + kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]} {} void HatchSubsystem::GrabHatch() { m_hatchSolenoid.Set(frc::DoubleSolenoid::kForward); diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/HatchSubsystem.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/HatchSubsystem.h index 0b6d256dfb..bb0610041e 100644 --- a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/HatchSubsystem.h +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/HatchSubsystem.h @@ -29,6 +29,5 @@ class HatchSubsystem : public frc2::SubsystemBase { private: // Components (e.g. motor controllers and sensors) should generally be // declared private and exposed only through public methods. - frc::PneumaticsControlModule m_pneumaticsModule; frc::DoubleSolenoid m_hatchSolenoid; }; diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/Robot.cpp index a679268094..303f74d6ec 100644 --- a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/Robot.cpp @@ -10,8 +10,6 @@ #include #include -#include "PneumaticsModule.h" - DriveTrain Robot::drivetrain; Pivot Robot::pivot; Collector Robot::collector; @@ -83,11 +81,6 @@ void Robot::Log() { drivetrain.GetRightEncoder().GetDistance()); } -frc::PneumaticsControlModule* pac::GetPneumatics() { - static frc::PneumaticsControlModule pcm; - return &pcm; -} - #ifndef RUNNING_FRC_TESTS int main() { return frc::StartRobot(); diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Pneumatics.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Pneumatics.cpp index 19163857f6..4c77f67849 100644 --- a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Pneumatics.cpp +++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Pneumatics.cpp @@ -6,8 +6,6 @@ #include -#include "PneumaticsModule.h" - Pneumatics::Pneumatics() : frc::Subsystem("Pneumatics") { AddChild("Pressure Sensor", m_pressureSensor); } @@ -18,7 +16,7 @@ void Pneumatics::InitDefaultCommand() { void Pneumatics::Start() { #ifndef SIMULATION - pac::GetPneumatics()->SetClosedLoopControl(true); + m_compressor.Start(); #endif } diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/Robot.h index 02dbec7f9b..5a07905282 100644 --- a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/Robot.h +++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/Robot.h @@ -4,7 +4,6 @@ #pragma once -#include #include #include #include diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Collector.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Collector.h index f493cbec7e..53fc0586f2 100644 --- a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Collector.h +++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Collector.h @@ -9,8 +9,6 @@ #include #include -#include "PneumaticsModule.h" - /** * The Collector subsystem has one motor for the rollers, a limit switch for * ball @@ -70,6 +68,6 @@ class Collector : public frc::Subsystem { // Subsystem devices frc::PWMSparkMax m_rollerMotor{6}; frc::DigitalInput m_ballDetector{10}; - frc::Solenoid m_piston{pac::GetPneumatics(), 1}; + frc::Solenoid m_piston{frc::PneumaticsModuleType::CTREPCM, 1}; frc::DigitalInput m_openDetector{6}; }; diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Pneumatics.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Pneumatics.h index ce1937202a..53111f95f6 100644 --- a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Pneumatics.h +++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Pneumatics.h @@ -5,6 +5,7 @@ #pragma once #include +#include #include /** @@ -41,5 +42,10 @@ class Pneumatics : public frc::Subsystem { private: frc::AnalogInput m_pressureSensor{3}; +#ifndef SIMULATION + frc::Compressor m_compressor{ + 1, frc::PneumaticsModuleType::CTREPCM}; // TODO: (1, 14, 1, 8); +#endif + static constexpr double kMaxPressure = 2.55; }; diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Shooter.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Shooter.h index f4f131852d..e52a97c693 100644 --- a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Shooter.h +++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Shooter.h @@ -9,8 +9,6 @@ #include #include -#include "PneumaticsModule.h" - /** * The Shooter subsystem handles shooting. The mechanism for shooting is * slightly complicated because it has to pneumatic cylinders for shooting, and @@ -116,9 +114,9 @@ class Shooter : public frc::Subsystem { private: // Devices - frc::DoubleSolenoid m_piston1{pac::GetPneumatics(), 3, 4}; - frc::DoubleSolenoid m_piston2{pac::GetPneumatics(), 5, 6}; - frc::Solenoid m_latchPiston{pac::GetPneumatics(), 2}; + frc::DoubleSolenoid m_piston1{frc::PneumaticsModuleType::CTREPCM, 3, 4}; + frc::DoubleSolenoid m_piston2{frc::PneumaticsModuleType::CTREPCM, 5, 6}; + frc::Solenoid m_latchPiston{1, frc::PneumaticsModuleType::CTREPCM, 2}; frc::DigitalInput m_piston1ReedSwitchFront{9}; frc::DigitalInput m_piston1ReedSwitchBack{11}; frc::DigitalInput m_hotGoalSensor{ diff --git a/wpilibcExamples/src/main/cpp/examples/Solenoid/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Solenoid/cpp/Robot.cpp index acbab4d6d5..82e481bffa 100644 --- a/wpilibcExamples/src/main/cpp/examples/Solenoid/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/Solenoid/cpp/Robot.cpp @@ -54,13 +54,12 @@ class Robot : public frc::TimedRobot { private: frc::Joystick m_stick{0}; - frc::PneumaticsControlModule m_pneumaticsModule; - // Solenoid corresponds to a single solenoid. - frc::Solenoid m_solenoid{m_pneumaticsModule, 0}; + frc::Solenoid m_solenoid{frc::PneumaticsModuleType::CTREPCM, 0}; // DoubleSolenoid corresponds to a double solenoid. - frc::DoubleSolenoid m_doubleSolenoid{m_pneumaticsModule, 1, 2}; + frc::DoubleSolenoid m_doubleSolenoid{frc::PneumaticsModuleType::CTREPCM, 1, + 2}; static constexpr int kSolenoidButton = 1; static constexpr int kDoubleSolenoidForward = 2; diff --git a/wpilibcIntegrationTests/src/main/native/cpp/PCMTest.cpp b/wpilibcIntegrationTests/src/main/native/cpp/PCMTest.cpp index 5851a507f7..1cc70b7c25 100644 --- a/wpilibcIntegrationTests/src/main/native/cpp/PCMTest.cpp +++ b/wpilibcIntegrationTests/src/main/native/cpp/PCMTest.cpp @@ -66,8 +66,10 @@ TEST_F(PCMTest, PressureSwitch) { */ TEST_F(PCMTest, Solenoid) { Reset(); - frc::Solenoid solenoid1{m_pneumaticsModule, TestBench::kSolenoidChannel1}; - frc::Solenoid solenoid2{m_pneumaticsModule, TestBench::kSolenoidChannel2}; + frc::Solenoid solenoid1{frc::PneumaticsModuleType::CTREPCM, + TestBench::kSolenoidChannel1}; + frc::Solenoid solenoid2{frc::PneumaticsModuleType::CTREPCM, + TestBench::kSolenoidChannel2}; // Turn both solenoids off solenoid1.Set(false); @@ -111,7 +113,8 @@ TEST_F(PCMTest, Solenoid) { * with the DoubleSolenoid class. */ TEST_F(PCMTest, DoubleSolenoid) { - frc::DoubleSolenoid solenoid{m_pneumaticsModule, TestBench::kSolenoidChannel1, + frc::DoubleSolenoid solenoid{frc::PneumaticsModuleType::CTREPCM, + TestBench::kSolenoidChannel1, TestBench::kSolenoidChannel2}; solenoid.Set(frc::DoubleSolenoid::kOff); @@ -138,8 +141,10 @@ TEST_F(PCMTest, DoubleSolenoid) { TEST_F(PCMTest, OneShot) { Reset(); - frc::Solenoid solenoid1{m_pneumaticsModule, TestBench::kSolenoidChannel1}; - frc::Solenoid solenoid2{m_pneumaticsModule, TestBench::kSolenoidChannel2}; + frc::Solenoid solenoid1{frc::PneumaticsModuleType::CTREPCM, + TestBench::kSolenoidChannel1}; + frc::Solenoid solenoid2{frc::PneumaticsModuleType::CTREPCM, + TestBench::kSolenoidChannel2}; // Turn both solenoids off solenoid1.Set(false); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java new file mode 100644 index 0000000000..f48fb63af8 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java @@ -0,0 +1,154 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +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; + +/** + * Class for operating a compressor connected to a pneumatics module. The module will automatically + * run in closed loop mode by default whenever a {@link Solenoid} object is created. For most cases, + * a Compressor object does not need to be instantiated or used in a robot program. This class is + * only required in cases where the robot program needs a more detailed status of the compressor or + * to enable/disable closed loop control. + * + *

Note: you cannot operate the compressor directly from this class as doing so would circumvent + * the safety provided by using the pressure switch and closed loop control. You can only turn off + * closed loop control, thereby stopping the compressor from operating. + */ +public class Compressor implements Sendable, AutoCloseable { + private PneumaticsBase m_module; + + /** + * Constructs a compressor for a specified module and type. + * + * @param module The module ID to use. + * @param moduleType The module type to use. + */ + public Compressor(int module, PneumaticsModuleType moduleType) { + m_module = PneumaticsBase.getForType(module, moduleType); + boolean allocatedCompressor = false; + boolean successfulCompletion = false; + + try { + if (!m_module.reserveCompressor()) { + throw new AllocationException("Compressor already allocated"); + } + + allocatedCompressor = true; + + m_module.setClosedLoopControl(true); + + HAL.report(tResourceType.kResourceType_Compressor, module + 1); + SendableRegistry.addLW(this, "Compressor", module); + successfulCompletion = true; + + } finally { + if (!successfulCompletion) { + if (allocatedCompressor) { + m_module.unreserveCompressor(); + } + m_module.close(); + } + } + } + + /** + * Constructs a compressor for a default module and specified type. + * + * @param moduleType The module type to use. + */ + public Compressor(PneumaticsModuleType moduleType) { + this(PneumaticsBase.getDefaultForType(moduleType), moduleType); + } + + @Override + public void close() { + SendableRegistry.remove(this); + m_module.unreserveCompressor(); + m_module.close(); + m_module = null; + } + + /** + * Start the compressor running in closed loop control mode. + * + *

Use the method in cases where you would like to manually stop and start the compressor for + * applications such as conserving battery or making sure that the compressor motor doesn't start + * during critical operations. + */ + public void start() { + setClosedLoopControl(true); + } + + /** + * Stop the compressor from running in closed loop control mode. + * + *

Use the method in cases where you would like to manually stop and start the compressor for + * applications such as conserving battery or making sure that the compressor motor doesn't start + * during critical operations. + */ + public void stop() { + setClosedLoopControl(false); + } + + /** + * Get the status of the compressor. + * + * @return true if the compressor is on + */ + public boolean enabled() { + return m_module.getCompressor(); + } + + /** + * Get the pressure switch value. + * + * @return true if the pressure is low + */ + public boolean getPressureSwitchValue() { + return m_module.getPressureSwitch(); + } + + /** + * Get the current being used by the compressor. + * + * @return current consumed by the compressor in amps + */ + public double getCompressorCurrent() { + return m_module.getCompressorCurrent(); + } + + /** + * Set the PCM in closed loop control mode. + * + * @param on if true sets the compressor to be in closed loop control mode (default) + */ + public void setClosedLoopControl(boolean on) { + m_module.setClosedLoopControl(on); + } + + /** + * Gets the current operating mode of the PCM. + * + * @return true if compressor is operating on closed-loop mode + */ + public boolean getClosedLoopControl() { + return m_module.getClosedLoopControl(); + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Compressor"); + builder.addBooleanProperty( + "Closed Loop Control", this::getClosedLoopControl, this::setClosedLoopControl); + builder.addBooleanProperty("Enabled", this::enabled, null); + builder.addBooleanProperty("Pressure switch", this::getPressureSwitchValue, null); + } +} 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 51cab0c75e..68a1bcaa36 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java @@ -10,10 +10,10 @@ 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; -import java.util.Objects; /** - * DoubleSolenoid class for running 2 channels of high voltage Digital Output on the PCM. + * DoubleSolenoid class for running 2 channels of high voltage Digital Output on the pneumatics + * module. * *

The DoubleSolenoid class is typically used for pneumatics solenoids that have two positions * controlled by two separate channels. @@ -34,22 +34,33 @@ public class DoubleSolenoid implements Sendable, AutoCloseable { private final int m_reverseChannel; /** - * Constructor. + * Constructs a double solenoid for a default module of a specific module type. + * + * @param moduleType The module type to use. + * @param forwardChannel The forward channel on the module to control. + * @param reverseChannel The reverse channel on the module to control. + */ + public DoubleSolenoid( + final PneumaticsModuleType moduleType, final int forwardChannel, final int reverseChannel) { + this(PneumaticsBase.getDefaultForType(moduleType), moduleType, forwardChannel, reverseChannel); + } + + /** + * Constructs a double solenoid for a specified module of a specific module type. * * @param module The module of the solenoid module to use. - * @param forwardChannel The forward channel on the module to control (0..7). - * @param reverseChannel The reverse channel on the module to control (0..7). + * @param moduleType The module type to use. + * @param forwardChannel The forward channel on the module to control. + * @param reverseChannel The reverse channel on the module to control. */ - public DoubleSolenoid(PneumaticsBase module, final int forwardChannel, final int reverseChannel) { - m_module = Objects.requireNonNull(module, "Module cannot be null"); - - if (!module.checkSolenoidChannel(forwardChannel)) { - throw new IllegalArgumentException("Channel " + forwardChannel + " out of range"); - } - - if (!module.checkSolenoidChannel(reverseChannel)) { - throw new IllegalArgumentException("Channel " + reverseChannel + " out of range"); - } + public DoubleSolenoid( + final int module, + final PneumaticsModuleType moduleType, + final int forwardChannel, + final int reverseChannel) { + m_module = PneumaticsBase.getForType(module, moduleType); + boolean allocatedSolenoids = false; + boolean successfulCompletion = false; m_forwardChannel = forwardChannel; m_reverseChannel = reverseChannel; @@ -58,29 +69,49 @@ 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"); + try { + if (!m_module.checkSolenoidChannel(forwardChannel)) { + throw new IllegalArgumentException("Channel " + forwardChannel + " out of range"); + } + + if (!m_module.checkSolenoidChannel(reverseChannel)) { + throw new IllegalArgumentException("Channel " + reverseChannel + " out of range"); + } + + int allocMask = m_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"); + } + } + allocatedSolenoids = true; + + HAL.report( + tResourceType.kResourceType_Solenoid, forwardChannel + 1, m_module.getModuleNumber() + 1); + HAL.report( + tResourceType.kResourceType_Solenoid, reverseChannel + 1, m_module.getModuleNumber() + 1); + SendableRegistry.addLW(this, "DoubleSolenoid", m_module.getModuleNumber(), forwardChannel); + successfulCompletion = true; + } finally { + if (!successfulCompletion) { + if (allocatedSolenoids) { + m_module.unreserveSolenoids(m_mask); + } + m_module.close(); } } - - HAL.report( - tResourceType.kResourceType_Solenoid, forwardChannel + 1, module.getModuleNumber() + 1); - HAL.report( - tResourceType.kResourceType_Solenoid, reverseChannel + 1, module.getModuleNumber() + 1); - SendableRegistry.addLW(this, "DoubleSolenoid", module.getModuleNumber(), forwardChannel); } @Override public synchronized void close() { SendableRegistry.remove(this); m_module.unreserveSolenoids(m_mask); + m_module.close(); 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 0d5d6d70e7..c893d48557 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsBase.java @@ -5,6 +5,33 @@ package edu.wpi.first.wpilibj; public interface PneumaticsBase extends AutoCloseable { + /** + * For internal use to get a module for a specific type. + * + * @param module module number + * @param type module type + * @return module + */ + static PneumaticsControlModule getForType(int module, PneumaticsModuleType type) { + if (type == PneumaticsModuleType.CTREPCM) { + return new PneumaticsControlModule(); + } + throw new IllegalArgumentException("Unknown module type"); + } + + /** + * For internal use to get the default for a specific type. + * + * @param type module type + * @return module default + */ + static int getDefaultForType(PneumaticsModuleType type) { + if (type == PneumaticsModuleType.CTREPCM) { + return SensorUtil.getDefaultCTREPCMModule(); + } + throw new IllegalArgumentException("Unknown module type"); + } + /** * Sets solenoids on a pneumatics module. * @@ -49,6 +76,16 @@ public interface PneumaticsBase extends AutoCloseable { */ void setOneShotDuration(int index, int durMs); + boolean getCompressor(); + + boolean getPressureSwitch(); + + double getCompressorCurrent(); + + void setClosedLoopControl(boolean on); + + boolean getClosedLoopControl(); + /** * Check if a solenoid channel is valid. * @@ -71,4 +108,17 @@ public interface PneumaticsBase extends AutoCloseable { * @param mask The solenoid mask to unreserve */ void unreserveSolenoids(int mask); + + boolean reserveCompressor(); + + void unreserveCompressor(); + + @Override + void close(); + + Solenoid makeSolenoid(int channel); + + DoubleSolenoid makeDoubleSolenoid(int forwardChannel, int reverseChannel); + + Compressor makeCompressor(); } 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 6d3446d066..fcb0ec8262 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsControlModule.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsControlModule.java @@ -5,16 +5,68 @@ 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; +import java.util.HashMap; +import java.util.Map; -public class PneumaticsControlModule implements PneumaticsBase, Sendable { +/** Module class for controlling a Cross The Road Electronics Pneumatics Control Module. */ +public class PneumaticsControlModule implements PneumaticsBase { + private static class DataStore implements AutoCloseable { + public final int m_module; + public final int m_handle; + private int m_refCount; + private int m_reservedMask; + private boolean m_compressorReserved; + private final Object m_reserveLock = new Object(); + + DataStore(int module) { + m_handle = CTREPCMJNI.initialize(module); + m_module = module; + m_handleMap.put(module, this); + } + + @Override + public void close() { + CTREPCMJNI.free(m_handle); + m_handleMap.remove(m_module); + } + + public void addRef() { + m_refCount++; + } + + public void removeRef() { + m_refCount--; + if (m_refCount == 0) { + this.close(); + } + } + } + + private static final Map m_handleMap = new HashMap<>(); + private static final Object m_handleLock = new Object(); + + private static DataStore getForModule(int module) { + synchronized (m_handleLock) { + Integer moduleBoxed = module; + DataStore pcm = m_handleMap.get(moduleBoxed); + if (pcm == null) { + pcm = new DataStore(module); + } + pcm.addRef(); + return pcm; + } + } + + private static void freeModule(DataStore store) { + synchronized (m_handleLock) { + store.removeRef(); + } + } + + private final DataStore m_dataStore; private final int m_handle; - private final int m_module; - private int m_reservedMask; - private final Object m_reserveLock = new Object(); + /** Constructs a PneumaticsControlModule with the default id (0). */ public PneumaticsControlModule() { this(SensorUtil.getDefaultCTREPCMModule()); } @@ -25,34 +77,36 @@ public class PneumaticsControlModule implements PneumaticsBase, Sendable { * @param module module number to construct */ public PneumaticsControlModule(int module) { - m_handle = CTREPCMJNI.initialize(module); - m_module = module; - - SendableRegistry.addLW(this, "Compressor", module); + m_dataStore = getForModule(module); + m_handle = m_dataStore.m_handle; } @Override public void close() { - CTREPCMJNI.free(m_handle); - SendableRegistry.remove(this); + freeModule(m_dataStore); } + @Override public boolean getCompressor() { return CTREPCMJNI.getCompressor(m_handle); } + @Override public void setClosedLoopControl(boolean enabled) { CTREPCMJNI.setClosedLoopControl(m_handle, enabled); } + @Override public boolean getClosedLoopControl() { return CTREPCMJNI.getClosedLoopControl(m_handle); } + @Override public boolean getPressureSwitch() { return CTREPCMJNI.getPressureSwitch(m_handle); } + @Override public double getCompressorCurrent() { return CTREPCMJNI.getCompressorCurrent(m_handle); } @@ -93,7 +147,7 @@ public class PneumaticsControlModule implements PneumaticsBase, Sendable { @Override public int getModuleNumber() { - return m_module; + return m_dataStore.m_handle; } @Override @@ -130,28 +184,53 @@ public class PneumaticsControlModule implements PneumaticsBase, Sendable { @Override public int checkAndReserveSolenoids(int mask) { - synchronized (m_reserveLock) { - if ((m_reservedMask & mask) != 0) { - return m_reservedMask & mask; + synchronized (m_dataStore.m_reserveLock) { + if ((m_dataStore.m_reservedMask & mask) != 0) { + return m_dataStore.m_reservedMask & mask; } - m_reservedMask |= mask; + m_dataStore.m_reservedMask |= mask; return 0; } } @Override public void unreserveSolenoids(int mask) { - synchronized (m_reserveLock) { - m_reservedMask &= ~mask; + synchronized (m_dataStore.m_reserveLock) { + m_dataStore.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); + public Solenoid makeSolenoid(int channel) { + return new Solenoid(m_dataStore.m_module, PneumaticsModuleType.CTREPCM, channel); + } + + @Override + public DoubleSolenoid makeDoubleSolenoid(int forwardChannel, int reverseChannel) { + return new DoubleSolenoid( + m_dataStore.m_module, PneumaticsModuleType.CTREPCM, forwardChannel, reverseChannel); + } + + @Override + public Compressor makeCompressor() { + return new Compressor(m_dataStore.m_module, PneumaticsModuleType.CTREPCM); + } + + @Override + public boolean reserveCompressor() { + synchronized (m_dataStore.m_reserveLock) { + if (m_dataStore.m_compressorReserved) { + return false; + } + m_dataStore.m_compressorReserved = true; + return true; + } + } + + @Override + public void unreserveCompressor() { + synchronized (m_dataStore.m_reserveLock) { + m_dataStore.m_compressorReserved = false; + } } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsModuleType.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsModuleType.java new file mode 100644 index 0000000000..a7951e8b5c --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsModuleType.java @@ -0,0 +1,10 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj; + +public enum PneumaticsModuleType { + CTREPCM, + REVPH; +} 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 bfa064af10..e86042b456 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java @@ -10,13 +10,12 @@ 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; -import java.util.Objects; /** - * Solenoid class for running high voltage Digital Output on the PCM. + * Solenoid class for running high voltage Digital Output on a pneumatics module. * *

The Solenoid class is typically used for pneumatic solenoids, but could be used for any device - * within the current spec of the PCM. + * within the current spec of the module. */ public class Solenoid implements Sendable, AutoCloseable { private final int m_mask; // The channel mask @@ -24,33 +23,60 @@ public class Solenoid implements Sendable, AutoCloseable { private PneumaticsBase m_module; /** - * Constructor. + * Constructs a solenoid for a default module and specified type. * - * @param module The PCM the solenoid is attached to. - * @param channel The channel on the PCM to control (0..7). + * @param moduleType The module type to use. + * @param channel The channel the solenoid is on. */ - public Solenoid(PneumaticsBase module, final int channel) { - m_module = Objects.requireNonNull(module, "Module cannot be null"); + public Solenoid(final PneumaticsModuleType moduleType, final int channel) { + this(PneumaticsBase.getDefaultForType(moduleType), moduleType, channel); + } - if (!module.checkSolenoidChannel(channel)) { - throw new IllegalArgumentException(); // TODO fix me - } + /** + * Constructs a solenoid for a specified module and type. + * + * @param module The module ID to use. + * @param moduleType The module type to use. + * @param channel The channel the solenoid is on. + */ + public Solenoid(final int module, final PneumaticsModuleType moduleType, final int channel) { + m_module = PneumaticsBase.getForType(module, moduleType); + boolean allocatedSolenoids = false; + boolean successfulCompletion = false; m_mask = 1 << channel; m_channel = channel; - if (module.checkAndReserveSolenoids(m_mask) != 0) { - throw new AllocationException("Solenoid already allocated"); - } + try { + if (!m_module.checkSolenoidChannel(channel)) { + throw new IllegalArgumentException("Channel " + channel + " out of range"); + } - HAL.report(tResourceType.kResourceType_Solenoid, channel + 1, module.getModuleNumber() + 1); - SendableRegistry.addLW(this, "Solenoid", module.getModuleNumber(), channel); + if (m_module.checkAndReserveSolenoids(m_mask) != 0) { + throw new AllocationException("Solenoid already allocated"); + } + + allocatedSolenoids = true; + + HAL.report(tResourceType.kResourceType_Solenoid, channel + 1, m_module.getModuleNumber() + 1); + SendableRegistry.addLW(this, "Solenoid", m_module.getModuleNumber(), channel); + successfulCompletion = true; + + } finally { + if (!successfulCompletion) { + if (allocatedSolenoids) { + m_module.unreserveSolenoids(m_mask); + } + m_module.close(); + } + } } @Override public void close() { SendableRegistry.remove(this); m_module.unreserveSolenoids(m_mask); + m_module.close(); m_module = null; } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/DoubleSolenoidTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/DoubleSolenoidTest.java index 717ae2e988..249c9461d1 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/DoubleSolenoidTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/DoubleSolenoidTest.java @@ -13,8 +13,7 @@ import org.junit.jupiter.api.Test; public class DoubleSolenoidTest { @Test void testValidInitialization() { - try (PneumaticsControlModule pcm = new PneumaticsControlModule(3); - DoubleSolenoid solenoid = new DoubleSolenoid(pcm, 2, 3)) { + try (DoubleSolenoid solenoid = new DoubleSolenoid(3, PneumaticsModuleType.CTREPCM, 2, 3)) { solenoid.set(DoubleSolenoid.Value.kReverse); assertEquals(DoubleSolenoid.Value.kReverse, solenoid.get()); @@ -28,36 +27,41 @@ public class DoubleSolenoidTest { @Test void testThrowForwardPortAlreadyInitialized() { - try (PneumaticsControlModule pcm = new PneumaticsControlModule(5); - // Single solenoid that is reused for forward port - Solenoid solenoid = new Solenoid(pcm, 2)) { - assertThrows(AllocationException.class, () -> new DoubleSolenoid(pcm, 2, 3)); + try ( + // Single solenoid that is reused for forward port + Solenoid solenoid = new Solenoid(5, PneumaticsModuleType.CTREPCM, 2)) { + assertThrows( + AllocationException.class, + () -> new DoubleSolenoid(5, PneumaticsModuleType.CTREPCM, 2, 3)); } } @Test void testThrowReversePortAlreadyInitialized() { - try (PneumaticsControlModule pcm = new PneumaticsControlModule(6); - // Single solenoid that is reused for forward port - Solenoid solenoid = new Solenoid(pcm, 3)) { - assertThrows(AllocationException.class, () -> new DoubleSolenoid(pcm, 2, 3)); + try ( + // Single solenoid that is reused for forward port + Solenoid solenoid = new Solenoid(6, PneumaticsModuleType.CTREPCM, 3)) { + assertThrows( + AllocationException.class, + () -> new DoubleSolenoid(6, PneumaticsModuleType.CTREPCM, 2, 3)); } } @Test void testThrowBothPortsAlreadyInitialized() { - try (PneumaticsControlModule pcm = new PneumaticsControlModule(6); - // Single solenoid that is reused for forward port - Solenoid solenoid0 = new Solenoid(pcm, 2); - Solenoid solenoid1 = new Solenoid(pcm, 3)) { - assertThrows(AllocationException.class, () -> new DoubleSolenoid(pcm, 2, 3)); + try ( + // Single solenoid that is reused for forward port + Solenoid solenoid0 = new Solenoid(6, PneumaticsModuleType.CTREPCM, 2); + Solenoid solenoid1 = new Solenoid(6, PneumaticsModuleType.CTREPCM, 3)) { + assertThrows( + AllocationException.class, + () -> new DoubleSolenoid(6, PneumaticsModuleType.CTREPCM, 2, 3)); } } @Test void testToggle() { - try (PneumaticsControlModule pcm = new PneumaticsControlModule(4); - DoubleSolenoid solenoid = new DoubleSolenoid(pcm, 2, 3)) { + try (DoubleSolenoid solenoid = new DoubleSolenoid(4, PneumaticsModuleType.CTREPCM, 2, 3)) { // Bootstrap it into reverse solenoid.set(DoubleSolenoid.Value.kReverse); @@ -76,15 +80,15 @@ public class DoubleSolenoidTest { @Test void testInvalidForwardPort() { - try (PneumaticsControlModule pcm = new PneumaticsControlModule(0)) { - assertThrows(IllegalArgumentException.class, () -> new DoubleSolenoid(pcm, 100, 1)); - } + assertThrows( + IllegalArgumentException.class, + () -> new DoubleSolenoid(0, PneumaticsModuleType.CTREPCM, 100, 1)); } @Test void testInvalidReversePort() { - try (PneumaticsControlModule pcm = new PneumaticsControlModule(0)) { - assertThrows(IllegalArgumentException.class, () -> new DoubleSolenoid(pcm, 0, 100)); - } + assertThrows( + IllegalArgumentException.class, + () -> new DoubleSolenoid(0, PneumaticsModuleType.CTREPCM, 0, 100)); } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/SolenoidTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/SolenoidTest.java index fcd1e3c6fc..687035dc3c 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/SolenoidTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/SolenoidTest.java @@ -15,8 +15,7 @@ import org.junit.jupiter.api.Test; public class SolenoidTest { @Test void testValidInitialization() { - try (PneumaticsControlModule pcm = new PneumaticsControlModule(3); - Solenoid solenoid = new Solenoid(pcm, 2)) { + try (Solenoid solenoid = new Solenoid(3, PneumaticsModuleType.CTREPCM, 2)) { assertEquals(2, solenoid.getChannel()); solenoid.set(true); @@ -29,31 +28,29 @@ public class SolenoidTest { @Test void testDoubleInitialization() { - try (PneumaticsControlModule pcm = new PneumaticsControlModule(3); - Solenoid solenoid = new Solenoid(pcm, 2)) { - assertThrows(AllocationException.class, () -> new Solenoid(pcm, 2)); + try (Solenoid solenoid = new Solenoid(3, PneumaticsModuleType.CTREPCM, 2)) { + assertThrows( + AllocationException.class, () -> new Solenoid(3, PneumaticsModuleType.CTREPCM, 2)); } } @Test void testDoubleInitializationFromDoubleSolenoid() { - try (PneumaticsControlModule pcm = new PneumaticsControlModule(3); - DoubleSolenoid solenoid = new DoubleSolenoid(pcm, 2, 3)) { - assertThrows(AllocationException.class, () -> new Solenoid(pcm, 2)); + try (DoubleSolenoid solenoid = new DoubleSolenoid(3, PneumaticsModuleType.CTREPCM, 2, 3)) { + assertThrows( + AllocationException.class, () -> new Solenoid(3, PneumaticsModuleType.CTREPCM, 2)); } } @Test void testInvalidChannel() { - try (PneumaticsControlModule pcm = new PneumaticsControlModule(3)) { - assertThrows(IllegalArgumentException.class, () -> new Solenoid(pcm, 100)); - } + assertThrows( + IllegalArgumentException.class, () -> new Solenoid(3, PneumaticsModuleType.CTREPCM, 100)); } @Test void testToggle() { - try (PneumaticsControlModule pcm = new PneumaticsControlModule(3); - Solenoid solenoid = new Solenoid(pcm, 2)) { + try (Solenoid solenoid = new Solenoid(3, PneumaticsModuleType.CTREPCM, 2)) { solenoid.set(true); assertTrue(solenoid.get()); diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/CTREPCMSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/CTREPCMSimTest.java index 6e2d289f6e..29cac3a426 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/CTREPCMSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/CTREPCMSimTest.java @@ -12,6 +12,7 @@ import static org.junit.jupiter.api.Assertions.assertTrue; import edu.wpi.first.hal.HAL; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.PneumaticsControlModule; +import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback; import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback; import org.junit.jupiter.api.Test; @@ -32,6 +33,7 @@ class CTREPCMSimTest { PneumaticsControlModule pcm = new PneumaticsControlModule(0)) { assertTrue(sim.getInitialized()); } + assertFalse(sim.getInitialized()); } @Test @@ -39,8 +41,8 @@ class CTREPCMSimTest { HAL.initialize(500, 0); try (PneumaticsControlModule pcm = new PneumaticsControlModule(0); - DoubleSolenoid doubleSolenoid = new DoubleSolenoid(pcm, 3, 4)) { - CTREPCMSim sim = new CTREPCMSim(pcm); + DoubleSolenoid doubleSolenoid = new DoubleSolenoid(0, PneumaticsModuleType.CTREPCM, 3, 4)) { + CTREPCMSim sim = new CTREPCMSim(0); sim.resetData(); BooleanCallback callback3 = new BooleanCallback(); @@ -75,11 +77,14 @@ class CTREPCMSimTest { // Off callback3.reset(); callback4.reset(); - doubleSolenoid.set(DoubleSolenoid.Value.kForward); - assertFalse(callback3.wasTriggered()); - assertNull(callback3.getSetValue()); + doubleSolenoid.set(DoubleSolenoid.Value.kOff); + assertTrue(callback3.wasTriggered()); + assertFalse(callback3.getSetValue()); assertFalse(callback4.wasTriggered()); assertNull(callback4.getSetValue()); + assertFalse(sim.getSolenoidOutput(3)); + assertFalse(sim.getSolenoidOutput(4)); + assertEquals(0x0, pcm.getSolenoids()); } } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java index 3ed9f7c85e..fc508f30ff 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java @@ -8,17 +8,15 @@ import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward; import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse; import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.PneumaticsControlModule; +import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.HatchConstants; import edu.wpi.first.wpilibj2.command.SubsystemBase; /** A hatch mechanism actuated by a single {@link edu.wpi.first.wpilibj.DoubleSolenoid}. */ public class HatchSubsystem extends SubsystemBase { - private final PneumaticsControlModule m_controlModule = - new PneumaticsControlModule(HatchConstants.kHatchSolenoidModule); private final DoubleSolenoid m_hatchSolenoid = new DoubleSolenoid( - m_controlModule, + PneumaticsModuleType.CTREPCM, HatchConstants.kHatchSolenoidPorts[0], HatchConstants.kHatchSolenoidPorts[1]); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java index 456d11113e..3526d17bc7 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java @@ -8,17 +8,15 @@ import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward; import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse; import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.PneumaticsControlModule; +import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.HatchConstants; import edu.wpi.first.wpilibj2.command.SubsystemBase; /** A hatch mechanism actuated by a single {@link DoubleSolenoid}. */ public class HatchSubsystem extends SubsystemBase { - private final PneumaticsControlModule m_controlModule = - new PneumaticsControlModule(HatchConstants.kHatchSolenoidModule); private final DoubleSolenoid m_hatchSolenoid = new DoubleSolenoid( - m_controlModule, + PneumaticsModuleType.CTREPCM, HatchConstants.kHatchSolenoidPorts[0], HatchConstants.kHatchSolenoidPorts[1]); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java index 6c87b76607..216a87f6b9 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java @@ -4,7 +4,6 @@ package edu.wpi.first.wpilibj.examples.pacgoat; -import edu.wpi.first.wpilibj.PneumaticsControlModule; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Scheduler; @@ -30,8 +29,6 @@ public class Robot extends TimedRobot { Command m_autonomousCommand; public static OI oi; - public static PneumaticsControlModule pneumaticsModule = new PneumaticsControlModule(1); - // Initialize the subsystems public static DriveTrain drivetrain = new DriveTrain(); public static Collector collector = new Collector(); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java index 588834d9e9..b9c0c585af 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java @@ -5,9 +5,9 @@ package edu.wpi.first.wpilibj.examples.pacgoat.subsystems; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.examples.pacgoat.Robot; import edu.wpi.first.wpilibj.motorcontrol.MotorController; import edu.wpi.first.wpilibj.motorcontrol.Victor; @@ -25,7 +25,7 @@ public class Collector extends Subsystem { private final MotorController m_rollerMotor = new Victor(6); private final DigitalInput m_ballDetector = new DigitalInput(10); private final DigitalInput m_openDetector = new DigitalInput(6); - private final Solenoid m_piston = new Solenoid(Robot.pneumaticsModule, 1); + private final Solenoid m_piston = new Solenoid(PneumaticsModuleType.CTREPCM, 1); /** Create a new collector subsystem. */ public Collector() { diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java index 45021ca6b0..5e7b28d893 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java @@ -6,9 +6,9 @@ package edu.wpi.first.wpilibj.examples.pacgoat.subsystems; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.examples.pacgoat.Robot; /** * The Shooter subsystem handles shooting. The mechanism for shooting is slightly complicated @@ -21,9 +21,9 @@ import edu.wpi.first.wpilibj.examples.pacgoat.Robot; */ public class Shooter extends Subsystem { // Devices - DoubleSolenoid m_piston1 = new DoubleSolenoid(Robot.pneumaticsModule, 3, 4); - DoubleSolenoid m_piston2 = new DoubleSolenoid(Robot.pneumaticsModule, 5, 6); - Solenoid m_latchPiston = new Solenoid(Robot.pneumaticsModule, 2); + DoubleSolenoid m_piston1 = new DoubleSolenoid(PneumaticsModuleType.CTREPCM, 3, 4); + DoubleSolenoid m_piston2 = new DoubleSolenoid(PneumaticsModuleType.CTREPCM, 5, 6); + Solenoid m_latchPiston = new Solenoid(PneumaticsModuleType.CTREPCM, 2); DigitalInput m_piston1ReedSwitchFront = new DigitalInput(9); DigitalInput m_piston1ReedSwitchBack = new DigitalInput(11); // NOTE: currently ignored in simulation diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Robot.java index 6db5ca24e5..98fb3b6afc 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Robot.java @@ -6,7 +6,7 @@ package edu.wpi.first.wpilibj.examples.solenoid; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.PneumaticsControlModule; +import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.TimedRobot; @@ -24,13 +24,12 @@ import edu.wpi.first.wpilibj.TimedRobot; public class Robot extends TimedRobot { private final Joystick m_stick = new Joystick(0); - private final PneumaticsControlModule m_pneumaticsModule = new PneumaticsControlModule(0); - // Solenoid corresponds to a single solenoid. - private final Solenoid m_solenoid = new Solenoid(m_pneumaticsModule, 0); + private final Solenoid m_solenoid = new Solenoid(PneumaticsModuleType.CTREPCM, 0); // DoubleSolenoid corresponds to a double solenoid. - private final DoubleSolenoid m_doubleSolenoid = new DoubleSolenoid(m_pneumaticsModule, 1, 2); + private final DoubleSolenoid m_doubleSolenoid = + new DoubleSolenoid(PneumaticsModuleType.CTREPCM, 1, 2); private static final int kSolenoidButton = 1; private static final int kDoubleSolenoidForward = 2; diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PCMTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PCMTest.java index cd6d0673ba..9c44983184 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PCMTest.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PCMTest.java @@ -99,8 +99,8 @@ public class PCMTest extends AbstractComsSetup { public void testSolenoid() throws Exception { reset(); - Solenoid solenoid1 = new Solenoid(pcm, 0); - Solenoid solenoid2 = new Solenoid(pcm, 1); + Solenoid solenoid1 = new Solenoid(PneumaticsModuleType.CTREPCM, 0); + Solenoid solenoid2 = new Solenoid(PneumaticsModuleType.CTREPCM, 1); solenoid1.set(false); solenoid2.set(false); @@ -147,7 +147,7 @@ public class PCMTest extends AbstractComsSetup { */ @Test public void doubleSolenoid() { - DoubleSolenoid solenoid = new DoubleSolenoid(pcm, 0, 1); + DoubleSolenoid solenoid = new DoubleSolenoid(PneumaticsModuleType.CTREPCM, 0, 1); solenoid.set(DoubleSolenoid.Value.kOff); Timer.delay(kSolenoidDelayTime); @@ -177,8 +177,8 @@ public class PCMTest extends AbstractComsSetup { public void testOneShot() throws Exception { reset(); - Solenoid solenoid1 = new Solenoid(pcm, 0); - Solenoid solenoid2 = new Solenoid(pcm, 1); + Solenoid solenoid1 = new Solenoid(PneumaticsModuleType.CTREPCM, 0); + Solenoid solenoid2 = new Solenoid(PneumaticsModuleType.CTREPCM, 1); solenoid1.set(false); solenoid2.set(false);