[wpilib] Make solenoids exclusive use, PCM act like old sendable compressor (#3464)

wpilibc/src/main/native/cpp/DoubleSolenoid.cpp

@@ -49,6 +49,20 @@ DoubleSolenoid::DoubleSolenoid(std::shared_ptr<PneumaticsBase> 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<PneumaticsBase> module,
                                m_module->GetModuleNumber(), m_forwardChannel);
 }
 
-DoubleSolenoid::~DoubleSolenoid() {}
+DoubleSolenoid::~DoubleSolenoid() {
+  m_module->UnreserveSolenoids(m_mask);
+}
 
 void DoubleSolenoid::Set(Value value) {
   int setValue = 0;

wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp

@@ -6,6 +6,8 @@
 
 #include <hal/CTREPCM.h>
 #include <wpi/StackTrace.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
 #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<uint32_t>(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<uint32_t>(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);
+}

wpilibc/src/main/native/cpp/Solenoid.cpp

@@ -34,13 +34,19 @@ Solenoid::Solenoid(std::shared_ptr<PneumaticsBase> 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;