[wpilib] Use PSI for compressor config and sensor reading (#3847)

This adds the REV Analog Pressure Sensor PSI to volt (and vice versa) conversion to allow setting the compressor config in PSI and getting the sensor reading in PSI. Also adds input validation for pressure values at the higher level.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpilibc/src/main/native/cpp/Compressor.cpp

@@ -56,6 +56,10 @@ units::volt_t Compressor::GetAnalogVoltage() const {
   return m_module->GetAnalogVoltage(0);
 }
 
+units::pounds_per_square_inch_t Compressor::GetPressure() const {
+  return m_module->GetPressure(0);
+}
+
 void Compressor::Disable() {
   m_module->DisableCompressor();
 }
@@ -64,14 +68,14 @@ void Compressor::EnableDigital() {
   m_module->EnableCompressorDigital();
 }
 
-void Compressor::EnableAnalog(units::volt_t minAnalogVoltage,
-                              units::volt_t maxAnalogVoltage) {
-  m_module->EnableCompressorAnalog(minAnalogVoltage, maxAnalogVoltage);
+void Compressor::EnableAnalog(units::pounds_per_square_inch_t minPressure,
+                              units::pounds_per_square_inch_t maxPressure) {
+  m_module->EnableCompressorAnalog(minPressure, maxPressure);
 }
 
-void Compressor::EnableHybrid(units::volt_t minAnalogVoltage,
-                              units::volt_t maxAnalogVoltage) {
-  m_module->EnableCompressorHybrid(minAnalogVoltage, maxAnalogVoltage);
+void Compressor::EnableHybrid(units::pounds_per_square_inch_t minPressure,
+                              units::pounds_per_square_inch_t maxPressure) {
+  m_module->EnableCompressorHybrid(minPressure, maxPressure);
 }
 
 CompressorConfigType Compressor::GetConfigType() const {

wpilibc/src/main/native/cpp/PneumaticHub.cpp

@@ -13,9 +13,24 @@
 #include "frc/Errors.h"
 #include "frc/SensorUtil.h"
 #include "frc/Solenoid.h"
+#include "frc/fmt/Units.h"
 
 using namespace frc;
 
+/** Converts volts to PSI per the REV Analog Pressure Sensor datasheet. */
+units::pounds_per_square_inch_t VoltsToPSI(units::volt_t sensorVoltage,
+                                           units::volt_t supplyVoltage) {
+  auto pressure = 250 * (sensorVoltage.value() / supplyVoltage.value()) - 25;
+  return units::pounds_per_square_inch_t{pressure};
+}
+
+/** Converts PSI to volts per the REV Analog Pressure Sensor datasheet. */
+units::volt_t PSIToVolts(units::pounds_per_square_inch_t pressure,
+                         units::volt_t supplyVoltage) {
+  auto voltage = supplyVoltage.value() * (0.004 * pressure.value() + 0.1);
+  return units::volt_t{voltage};
+}
+
 wpi::mutex PneumaticHub::m_handleLock;
 std::unique_ptr<wpi::DenseMap<int, std::weak_ptr<PneumaticHub::DataStore>>>
     PneumaticHub::m_handleMap = nullptr;
@@ -93,17 +108,51 @@ void PneumaticHub::EnableCompressorDigital() {
   FRC_ReportError(status, "Module {}", m_module);
 }
 
-void PneumaticHub::EnableCompressorAnalog(units::volt_t minAnalogVoltage,
-                                          units::volt_t maxAnalogVoltage) {
+void PneumaticHub::EnableCompressorAnalog(
+    units::pounds_per_square_inch_t minPressure,
+    units::pounds_per_square_inch_t maxPressure) {
+  if (minPressure >= maxPressure) {
+    throw FRC_MakeError(err::InvalidParameter, "{}",
+                        "maxPressure must be greater than minPresure");
+  }
+  if (minPressure < 0_psi || minPressure > 120_psi) {
+    throw FRC_MakeError(err::ParameterOutOfRange,
+                        "minPressure must be between 0 and 120 PSI, got {}",
+                        minPressure);
+  }
+  if (maxPressure < 0_psi || maxPressure > 120_psi) {
+    throw FRC_MakeError(err::ParameterOutOfRange,
+                        "maxPressure must be between 0 and 120 PSI, got {}",
+                        maxPressure);
+  }
   int32_t status = 0;
+  units::volt_t minAnalogVoltage = PSIToVolts(minPressure, 5_V);
+  units::volt_t maxAnalogVoltage = PSIToVolts(maxPressure, 5_V);
   HAL_SetREVPHClosedLoopControlAnalog(m_handle, minAnalogVoltage.value(),
                                       maxAnalogVoltage.value(), &status);
   FRC_ReportError(status, "Module {}", m_module);
 }
 
-void PneumaticHub::EnableCompressorHybrid(units::volt_t minAnalogVoltage,
-                                          units::volt_t maxAnalogVoltage) {
+void PneumaticHub::EnableCompressorHybrid(
+    units::pounds_per_square_inch_t minPressure,
+    units::pounds_per_square_inch_t maxPressure) {
+  if (minPressure >= maxPressure) {
+    throw FRC_MakeError(err::InvalidParameter, "{}",
+                        "maxPressure must be greater than minPresure");
+  }
+  if (minPressure < 0_psi || minPressure > 120_psi) {
+    throw FRC_MakeError(err::ParameterOutOfRange,
+                        "minPressure must be between 0 and 120 PSI, got {}",
+                        minPressure);
+  }
+  if (maxPressure < 0_psi || maxPressure > 120_psi) {
+    throw FRC_MakeError(err::ParameterOutOfRange,
+                        "maxPressure must be between 0 and 120 PSI, got {}",
+                        maxPressure);
+  }
   int32_t status = 0;
+  units::volt_t minAnalogVoltage = PSIToVolts(minPressure, 5_V);
+  units::volt_t maxAnalogVoltage = PSIToVolts(maxPressure, 5_V);
   HAL_SetREVPHClosedLoopControlHybrid(m_handle, minAnalogVoltage.value(),
                                       maxAnalogVoltage.value(), &status);
   FRC_ReportError(status, "Module {}", m_module);
@@ -286,6 +335,15 @@ units::volt_t PneumaticHub::GetAnalogVoltage(int channel) const {
   return units::volt_t{voltage};
 }
 
+units::pounds_per_square_inch_t PneumaticHub::GetPressure(int channel) const {
+  int32_t status = 0;
+  auto sensorVoltage = HAL_GetREVPHAnalogVoltage(m_handle, channel, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  auto supplyVoltage = HAL_GetREVPH5VVoltage(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return VoltsToPSI(units::volt_t{sensorVoltage}, units::volt_t{supplyVoltage});
+}
+
 Solenoid PneumaticHub::MakeSolenoid(int channel) {
   return Solenoid{m_module, PneumaticsModuleType::REVPH, channel};
 }

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

@@ -97,14 +97,16 @@ void PneumaticsControlModule::EnableCompressorDigital() {
 }
 
 void PneumaticsControlModule::EnableCompressorAnalog(
-    units::volt_t minAnalogVoltage, units::volt_t maxAnalogVoltage) {
+    units::pounds_per_square_inch_t minPressure,
+    units::pounds_per_square_inch_t maxPressure) {
   int32_t status = 0;
   HAL_SetCTREPCMClosedLoopControl(m_handle, true, &status);
   FRC_CheckErrorStatus(status, "Module {}", m_module);
 }
 
 void PneumaticsControlModule::EnableCompressorHybrid(
-    units::volt_t minAnalogVoltage, units::volt_t maxAnalogVoltage) {
+    units::pounds_per_square_inch_t minPressure,
+    units::pounds_per_square_inch_t maxPressure) {
   int32_t status = 0;
   HAL_SetCTREPCMClosedLoopControl(m_handle, true, &status);
   FRC_CheckErrorStatus(status, "Module {}", m_module);
@@ -265,6 +267,11 @@ units::volt_t PneumaticsControlModule::GetAnalogVoltage(int channel) const {
   return units::volt_t{0};
 }
 
+units::pounds_per_square_inch_t PneumaticsControlModule::GetPressure(
+    int channel) const {
+  return 0_psi;
+}
+
 Solenoid PneumaticsControlModule::MakeSolenoid(int channel) {
   return Solenoid{m_module, PneumaticsModuleType::CTREPCM, channel};
 }

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

@@ -104,6 +104,14 @@ class Compressor : public wpi::Sendable,
    */
   units::volt_t GetAnalogVoltage() const;
 
+  /**
+   * Query the analog sensor pressure (on channel 0) (if supported). Note this
+   * is only for use with the REV Analog Pressure Sensor.
+   *
+   * @return The analog sensor pressure, in PSI
+   */
+  units::pounds_per_square_inch_t GetPressure() const;
+
   /**
    * Disable the compressor.
    */
@@ -115,26 +123,28 @@ class Compressor : public wpi::Sendable,
   void EnableDigital();
 
   /**
-   * Enable compressor closed loop control using analog input.
+   * Enable compressor closed loop control using analog input. Note this is only
+   * for use with the REV Analog Pressure Sensor.
    *
    * <p>On CTRE PCM, this will enable digital control.
    *
-   * @param minAnalogVoltage The minimum voltage to enable compressor
-   * @param maxAnalogVoltage The maximum voltage to disable compressor
+   * @param minPressure The minimum pressure in PSI to enable compressor
+   * @param maxPressure The maximum pressure in PSI to disable compressor
    */
-  void EnableAnalog(units::volt_t minAnalogVoltage,
-                    units::volt_t maxAnalogVoltage);
+  void EnableAnalog(units::pounds_per_square_inch_t minPressure,
+                    units::pounds_per_square_inch_t maxPressure);
 
   /**
-   * Enable compressor closed loop control using hybrid input.
+   * Enable compressor closed loop control using hybrid input. Note this is only
+   * for use with the REV Analog Pressure Sensor.
    *
    * On CTRE PCM, this will enable digital control.
    *
-   * @param minAnalogVoltage The minimum voltage to enable compressor
-   * @param maxAnalogVoltage The maximum voltage to disable compressor
+   * @param minPressure The minimum pressure in PSI to enable compressor
+   * @param maxPressure The maximum pressure in PSI to disable compressor
    */
-  void EnableHybrid(units::volt_t minAnalogVoltage,
-                    units::volt_t maxAnalogVoltage);
+  void EnableHybrid(units::pounds_per_square_inch_t minPressure,
+                    units::pounds_per_square_inch_t maxPressure);
 
   CompressorConfigType GetConfigType() const;
 

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

@@ -26,11 +26,13 @@ class PneumaticHub : public PneumaticsBase {
 
   void EnableCompressorDigital() override;
 
-  void EnableCompressorAnalog(units::volt_t minAnalogVoltage,
-                              units::volt_t maxAnalogVoltage) override;
+  void EnableCompressorAnalog(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) override;
 
-  void EnableCompressorHybrid(units::volt_t minAnalogVoltage,
-                              units::volt_t maxAnalogVoltage) override;
+  void EnableCompressorHybrid(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) override;
 
   CompressorConfigType GetCompressorConfigType() const override;
 
@@ -127,6 +129,8 @@ class PneumaticHub : public PneumaticsBase {
 
   units::volt_t GetAnalogVoltage(int channel) const override;
 
+  units::pounds_per_square_inch_t GetPressure(int channel) const override;
+
  private:
   class DataStore;
   friend class DataStore;

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

@@ -7,6 +7,7 @@
 #include <memory>
 
 #include <units/current.h>
+#include <units/pressure.h>
 #include <units/time.h>
 #include <units/voltage.h>
 
@@ -31,11 +32,13 @@ class PneumaticsBase {
 
   virtual void EnableCompressorDigital() = 0;
 
-  virtual void EnableCompressorAnalog(units::volt_t minAnalogVoltage,
-                                      units::volt_t maxAnalogVoltage) = 0;
+  virtual void EnableCompressorAnalog(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) = 0;
 
-  virtual void EnableCompressorHybrid(units::volt_t minAnalogVoltage,
-                                      units::volt_t maxAnalogVoltage) = 0;
+  virtual void EnableCompressorHybrid(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) = 0;
 
   virtual CompressorConfigType GetCompressorConfigType() const = 0;
 
@@ -63,6 +66,8 @@ class PneumaticsBase {
 
   virtual units::volt_t GetAnalogVoltage(int channel) const = 0;
 
+  virtual units::pounds_per_square_inch_t GetPressure(int channel) const = 0;
+
   virtual Solenoid MakeSolenoid(int channel) = 0;
   virtual DoubleSolenoid MakeDoubleSolenoid(int forwardChannel,
                                             int reverseChannel) = 0;

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

@@ -26,11 +26,13 @@ class PneumaticsControlModule : public PneumaticsBase {
 
   void EnableCompressorDigital() override;
 
-  void EnableCompressorAnalog(units::volt_t minAnalogVoltage,
-                              units::volt_t maxAnalogVoltage) override;
+  void EnableCompressorAnalog(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) override;
 
-  void EnableCompressorHybrid(units::volt_t minAnalogVoltage,
-                              units::volt_t maxAnalogVoltage) override;
+  void EnableCompressorHybrid(
+      units::pounds_per_square_inch_t minPressure,
+      units::pounds_per_square_inch_t maxPressure) override;
 
   CompressorConfigType GetCompressorConfigType() const override;
 
@@ -74,6 +76,8 @@ class PneumaticsControlModule : public PneumaticsBase {
 
   units::volt_t GetAnalogVoltage(int channel) const override;
 
+  units::pounds_per_square_inch_t GetPressure(int channel) const override;
+
   Solenoid MakeSolenoid(int channel) override;
   DoubleSolenoid MakeDoubleSolenoid(int forwardChannel,
                                     int reverseChannel) override;

wpilibc/src/test/native/cpp/simulation/REVPHSimTest.cpp

@@ -129,7 +129,7 @@ TEST(REVPHSimTest, SetEnableAnalog) {
   ph.DisableCompressor();
   EXPECT_EQ(ph.GetCompressorConfigType(), CompressorConfigType::Disabled);
 
-  ph.EnableCompressorAnalog(1_V, 2_V);
+  ph.EnableCompressorAnalog(1_psi, 2_psi);
   EXPECT_EQ(sim.GetCompressorConfigType(),
             static_cast<int>(CompressorConfigType::Analog));
   EXPECT_EQ(ph.GetCompressorConfigType(), CompressorConfigType::Analog);
@@ -150,7 +150,7 @@ TEST(REVPHSimTest, SetEnableHybrid) {
   ph.DisableCompressor();
   EXPECT_EQ(ph.GetCompressorConfigType(), CompressorConfigType::Disabled);
 
-  ph.EnableCompressorHybrid(1_V, 2_V);
+  ph.EnableCompressorHybrid(1_psi, 2_psi);
   EXPECT_EQ(sim.GetCompressorConfigType(),
             static_cast<int>(CompressorConfigType::Hybrid));
   EXPECT_EQ(ph.GetCompressorConfigType(), CompressorConfigType::Hybrid);

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

@@ -126,6 +126,16 @@ public class Compressor implements Sendable, AutoCloseable {
     return m_module.getAnalogVoltage(0);
   }
 
+  /**
+   * Query the analog sensor pressure (on channel 0) (if supported). Note this is only for use with
+   * the REV Analog Pressure Sensor.
+   *
+   * @return The analog sensor pressure, in PSI
+   */
+  public double getPressure() {
+    return m_module.getPressure(0);
+  }
+
   /** Disable the compressor. */
   public void disable() {
     m_module.disableCompressor();
@@ -137,27 +147,29 @@ public class Compressor implements Sendable, AutoCloseable {
   }
 
   /**
-   * Enable compressor closed loop control using analog input.
+   * Enable compressor closed loop control using analog input. Note this is only for use with the
+   * REV Analog Pressure Sensor.
    *
    * <p>On CTRE PCM, this will enable digital control.
    *
-   * @param minAnalogVoltage The minimum voltage to enable compressor
-   * @param maxAnalogVoltage The maximum voltage to disable compressor
+   * @param minPressure The minimum pressure in PSI to enable compressor
+   * @param maxPressure The maximum pressure in PSI to disable compressor
    */
-  public void enableAnalog(double minAnalogVoltage, double maxAnalogVoltage) {
-    m_module.enableCompressorAnalog(minAnalogVoltage, maxAnalogVoltage);
+  public void enableAnalog(double minPressure, double maxPressure) {
+    m_module.enableCompressorAnalog(minPressure, maxPressure);
   }
 
   /**
-   * Enable compressor closed loop control using hybrid input.
+   * Enable compressor closed loop control using hybrid input. Note this is only for use with the
+   * REV Analog Pressure Sensor.
    *
    * <p>On CTRE PCM, this will enable digital control.
    *
-   * @param minAnalogVoltage The minimum voltage to enable compressor
-   * @param maxAnalogVoltage The maximum voltage to disable compressor
+   * @param minPressure The minimum pressure in PSI to enable compressor
+   * @param maxPressure The maximum pressure in PSI to disable compressor
    */
-  public void enableHybrid(double minAnalogVoltage, double maxAnalogVoltage) {
-    m_module.enableCompressorHybrid(minAnalogVoltage, maxAnalogVoltage);
+  public void enableHybrid(double minPressure, double maxPressure) {
+    m_module.enableCompressorHybrid(minPressure, maxPressure);
   }
 
   /**

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

@@ -68,6 +68,18 @@ public class PneumaticHub implements PneumaticsBase {
     }
   }
 
+  /** Converts volts to PSI per the REV Analog Pressure Sensor datasheet. */
+  private static double voltsToPsi(double sensorVoltage, double supplyVoltage) {
+    double pressure = 250 * (sensorVoltage / supplyVoltage) - 25;
+    return pressure;
+  }
+
+  /** Converts PSI to volts per the REV Analog Pressure Sensor datasheet. */
+  private static double psiToVolts(double pressure, double supplyVoltage) {
+    double voltage = supplyVoltage * (0.004 * pressure + 0.1);
+    return voltage;
+  }
+
   private final DataStore m_dataStore;
   private final int m_handle;
 
@@ -210,12 +222,38 @@ public class PneumaticHub implements PneumaticsBase {
   }
 
   @Override
-  public void enableCompressorAnalog(double minAnalogVoltage, double maxAnalogVoltage) {
+  public void enableCompressorAnalog(double minPressure, double maxPressure) {
+    if (minPressure >= maxPressure) {
+      throw new IllegalArgumentException("maxPressure must be greater than minPressure");
+    }
+    if (minPressure < 0 || minPressure > 120) {
+      throw new IllegalArgumentException(
+          "minPressure must be between 0 and 120 PSI, got " + minPressure);
+    }
+    if (maxPressure < 0 || maxPressure > 120) {
+      throw new IllegalArgumentException(
+          "maxPressure must be between 0 and 120 PSI, got " + maxPressure);
+    }
+    double minAnalogVoltage = psiToVolts(minPressure, 5);
+    double maxAnalogVoltage = psiToVolts(maxPressure, 5);
     REVPHJNI.setClosedLoopControlAnalog(m_handle, minAnalogVoltage, maxAnalogVoltage);
   }
 
   @Override
-  public void enableCompressorHybrid(double minAnalogVoltage, double maxAnalogVoltage) {
+  public void enableCompressorHybrid(double minPressure, double maxPressure) {
+    if (minPressure >= maxPressure) {
+      throw new IllegalArgumentException("maxPressure must be greater than minPressure");
+    }
+    if (minPressure < 0 || minPressure > 120) {
+      throw new IllegalArgumentException(
+          "minPressure must be between 0 and 120 PSI, got " + minPressure);
+    }
+    if (maxPressure < 0 || maxPressure > 120) {
+      throw new IllegalArgumentException(
+          "maxPressure must be between 0 and 120 PSI, got " + maxPressure);
+    }
+    double minAnalogVoltage = psiToVolts(minPressure, 5);
+    double maxAnalogVoltage = psiToVolts(maxPressure, 5);
     REVPHJNI.setClosedLoopControlHybrid(m_handle, minAnalogVoltage, maxAnalogVoltage);
   }
 
@@ -224,6 +262,13 @@ public class PneumaticHub implements PneumaticsBase {
     return REVPHJNI.getAnalogVoltage(m_handle, channel);
   }
 
+  @Override
+  public double getPressure(int channel) {
+    double sensorVoltage = REVPHJNI.getAnalogVoltage(m_handle, channel);
+    double supplyVoltage = REVPHJNI.get5VVoltage(m_handle);
+    return voltsToPsi(sensorVoltage, supplyVoltage);
+  }
+
   void clearStickyFaults() {
     REVPHJNI.clearStickyFaults(m_handle);
   }

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

@@ -90,12 +90,14 @@ public interface PneumaticsBase extends AutoCloseable {
 
   void enableCompressorDigital();
 
-  void enableCompressorAnalog(double minAnalogVoltage, double maxAnalogVoltage);
+  void enableCompressorAnalog(double minPressure, double maxPressure);
 
-  void enableCompressorHybrid(double minAnalogVoltage, double maxAnalogVoltage);
+  void enableCompressorHybrid(double minPressure, double maxPressure);
 
   double getAnalogVoltage(int channel);
 
+  double getPressure(int channel);
+
   CompressorConfigType getCompressorConfigType();
 
   /**

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

@@ -235,12 +235,12 @@ public class PneumaticsControlModule implements PneumaticsBase {
   }
 
   @Override
-  public void enableCompressorAnalog(double minAnalogVoltage, double maxAnalogVoltage) {
+  public void enableCompressorAnalog(double minPressure, double maxPressure) {
     CTREPCMJNI.setClosedLoopControl(m_handle, false);
   }
 
   @Override
-  public void enableCompressorHybrid(double minAnalogVoltage, double maxAnalogVoltage) {
+  public void enableCompressorHybrid(double minPressure, double maxPressure) {
     CTREPCMJNI.setClosedLoopControl(m_handle, false);
   }
 
@@ -255,4 +255,9 @@ public class PneumaticsControlModule implements PneumaticsBase {
   public double getAnalogVoltage(int channel) {
     return 0;
   }
+
+  @Override
+  public double getPressure(int channel) {
+    return 0;
+  }
 }