[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;

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

wpilibc/src/main/native/include/frc/PneumaticsControlModule.h

@@ -5,20 +5,23 @@
 #pragma once
 
 #include <hal/Types.h>
+#include <wpi/mutex.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
 #include "PneumaticsBase.h"
 
 namespace frc {
-class PneumaticsControlModule : public PneumaticsBase {
+class PneumaticsControlModule
+    : public PneumaticsBase,
+      public wpi::Sendable,
+      public wpi::SendableHelper<PneumaticsControlModule> {
  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<HAL_CTREPCMHandle> m_handle;
+  uint32_t m_reservedMask;
+  wpi::mutex m_reservedLock;
 };
 }  // namespace frc