[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

wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java

@@ -6,6 +6,7 @@ package edu.wpi.first.wpilibj;
 
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.util.AllocationException;
 import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.util.sendable.SendableRegistry;
@@ -42,7 +43,13 @@ public class DoubleSolenoid implements Sendable, AutoCloseable {
   public DoubleSolenoid(PneumaticsBase module, final int forwardChannel, final int reverseChannel) {
     m_module = Objects.requireNonNull(module, "Module cannot be null");
 
-    // TODO check channels
+    if (!module.checkSolenoidChannel(forwardChannel)) {
+      throw new IllegalArgumentException("Channel " + forwardChannel + " out of range");
+    }
+
+    if (!module.checkSolenoidChannel(reverseChannel)) {
+      throw new IllegalArgumentException("Channel " + reverseChannel + " out of range");
+    }
 
     m_forwardChannel = forwardChannel;
     m_reverseChannel = reverseChannel;
@@ -51,6 +58,23 @@ public class DoubleSolenoid implements Sendable, AutoCloseable {
     m_reverseMask = 1 << reverseChannel;
     m_mask = m_forwardMask | m_reverseMask;
 
+    int allocMask = module.checkAndReserveSolenoids(m_mask);
+    if (allocMask != 0) {
+      if (allocMask == m_mask) {
+        throw new AllocationException(
+            "Channels " + forwardChannel + " and " + reverseChannel + " already allocated");
+      } else if (allocMask == m_forwardMask) {
+        throw new AllocationException("Channel " + forwardChannel + " already allocated");
+      } else {
+        throw new AllocationException("Channel " + reverseChannel + " already allocated");
+      }
+    }
+
+    if (module.checkAndReserveSolenoids(m_mask) != 0) {
+
+      throw new AllocationException("Solenoid(s) already allocated");
+    }
+
     HAL.report(
         tResourceType.kResourceType_Solenoid, forwardChannel + 1, module.getModuleNumber() + 1);
     HAL.report(
@@ -61,6 +85,7 @@ public class DoubleSolenoid implements Sendable, AutoCloseable {
   @Override
   public synchronized void close() {
     SendableRegistry.remove(this);
+    m_module.unreserveSolenoids(m_mask);
     m_module = null;
   }
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsBase.java

@@ -49,5 +49,26 @@ public interface PneumaticsBase extends AutoCloseable {
    */
   void setOneShotDuration(int index, int durMs);
 
+  /**
+   * Check if a solenoid channel is valid.
+   *
+   * @param channel Channel to check
+   * @return True if channel exists
+   */
   boolean checkSolenoidChannel(int channel);
+
+  /**
+   * Check to see if the masked solenoids can be reserved, and if not reserve them.
+   *
+   * @param mask The solenoid mask to reserve
+   * @return 0 if successful, mask of solenoids that couldn't be allocated otherwise
+   */
+  int checkAndReserveSolenoids(int mask);
+
+  /**
+   * Unreserve the masked solenoids.
+   *
+   * @param mask The solenoid mask to unreserve
+   */
+  void unreserveSolenoids(int mask);
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsControlModule.java

@@ -5,23 +5,36 @@
 package edu.wpi.first.wpilibj;
 
 import edu.wpi.first.hal.CTREPCMJNI;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 
-public class PneumaticsControlModule implements PneumaticsBase {
+public class PneumaticsControlModule implements PneumaticsBase, Sendable {
   private final int m_handle;
   private final int m_module;
+  private int m_reservedMask;
+  private final Object m_reserveLock = new Object();
 
   public PneumaticsControlModule() {
     this(SensorUtil.getDefaultCTREPCMModule());
   }
 
+  /**
+   * Constructs a PneumaticsControlModule.
+   *
+   * @param module module number to construct
+   */
   public PneumaticsControlModule(int module) {
     m_handle = CTREPCMJNI.initialize(module);
     m_module = module;
+
+    SendableRegistry.addLW(this, "Compressor", module);
   }
 
   @Override
   public void close() throws Exception {
     CTREPCMJNI.free(m_handle);
+    SendableRegistry.remove(this);
   }
 
   public boolean getCompressor() {
@@ -114,4 +127,31 @@ public class PneumaticsControlModule implements PneumaticsBase {
   public boolean checkSolenoidChannel(int channel) {
     return CTREPCMJNI.checkSolenoidChannel(channel);
   }
+
+  @Override
+  public int checkAndReserveSolenoids(int mask) {
+    synchronized (m_reserveLock) {
+      if ((m_reservedMask & mask) != 0) {
+        return m_reservedMask & mask;
+      }
+      m_reservedMask |= mask;
+      return 0;
+    }
+  }
+
+  @Override
+  public void unreserveSolenoids(int mask) {
+    synchronized (m_reserveLock) {
+      m_reservedMask &= ~mask;
+    }
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Compressor");
+    builder.addBooleanProperty(
+        "Closed Loop Control", this::getClosedLoopControl, this::setClosedLoopControl);
+    builder.addBooleanProperty("Enabled", this::getCompressor, null);
+    builder.addBooleanProperty("Pressure switch", this::getPressureSwitch, null);
+  }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java

@@ -6,6 +6,7 @@ package edu.wpi.first.wpilibj;
 
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.util.AllocationException;
 import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.util.sendable.SendableRegistry;
@@ -38,6 +39,10 @@ public class Solenoid implements Sendable, AutoCloseable {
     m_mask = 1 << channel;
     m_channel = channel;
 
+    if (module.checkAndReserveSolenoids(m_mask) != 0) {
+      throw new AllocationException("Solenoid already allocated");
+    }
+
     HAL.report(tResourceType.kResourceType_Solenoid, channel + 1, module.getModuleNumber() + 1);
     SendableRegistry.addLW(this, "Solenoid", module.getModuleNumber(), channel);
   }
@@ -45,6 +50,7 @@ public class Solenoid implements Sendable, AutoCloseable {
   @Override
   public void close() {
     SendableRegistry.remove(this);
+    m_module.unreserveSolenoids(m_mask);
     m_module = null;
   }