001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.wpilibj;
006
007import edu.wpi.first.hal.PortsJNI;
008import edu.wpi.first.hal.REVPHFaults;
009import edu.wpi.first.hal.REVPHJNI;
010import edu.wpi.first.hal.REVPHStickyFaults;
011import edu.wpi.first.hal.REVPHVersion;
012import java.util.HashMap;
013import java.util.Map;
014
015/** Module class for controlling a REV Robotics Pneumatic Hub. */
016public class PneumaticHub implements PneumaticsBase {
017  private static class DataStore implements AutoCloseable {
018    public final int m_module;
019    public final int m_handle;
020    private int m_refCount;
021    private int m_reservedMask;
022    private boolean m_compressorReserved;
023    public int[] m_oneShotDurMs = new int[PortsJNI.getNumREVPHChannels()];
024    private final Object m_reserveLock = new Object();
025
026    DataStore(int module) {
027      m_handle = REVPHJNI.initialize(module);
028      m_module = module;
029      m_handleMap.put(module, this);
030
031      final REVPHVersion version = REVPHJNI.getVersion(m_handle);
032      if (version.firmwareMajor > 0 && version.firmwareMajor < 22) {
033        final String fwVersion =
034            version.firmwareMajor + "." + version.firmwareMinor + "." + version.firmwareFix;
035        throw new IllegalStateException(
036            "The Pneumatic Hub has firmware version "
037                + fwVersion
038                + ", and must be updated to version 2022.0.0 or later "
039                + "using the REV Hardware Client.");
040      }
041    }
042
043    @Override
044    public void close() {
045      REVPHJNI.free(m_handle);
046      m_handleMap.remove(m_module);
047    }
048
049    public void addRef() {
050      m_refCount++;
051    }
052
053    public void removeRef() {
054      m_refCount--;
055      if (m_refCount == 0) {
056        this.close();
057      }
058    }
059  }
060
061  private static final Map<Integer, DataStore> m_handleMap = new HashMap<>();
062  private static final Object m_handleLock = new Object();
063
064  private static DataStore getForModule(int module) {
065    synchronized (m_handleLock) {
066      Integer moduleBoxed = module;
067      DataStore pcm = m_handleMap.get(moduleBoxed);
068      if (pcm == null) {
069        pcm = new DataStore(module);
070      }
071      pcm.addRef();
072      return pcm;
073    }
074  }
075
076  private static void freeModule(DataStore store) {
077    synchronized (m_handleLock) {
078      store.removeRef();
079    }
080  }
081
082  /** Converts volts to PSI per the REV Analog Pressure Sensor datasheet. */
083  private static double voltsToPsi(double sensorVoltage, double supplyVoltage) {
084    double pressure = 250 * (sensorVoltage / supplyVoltage) - 25;
085    return pressure;
086  }
087
088  /** Converts PSI to volts per the REV Analog Pressure Sensor datasheet. */
089  private static double psiToVolts(double pressure, double supplyVoltage) {
090    double voltage = supplyVoltage * (0.004 * pressure + 0.1);
091    return voltage;
092  }
093
094  private final DataStore m_dataStore;
095  private final int m_handle;
096
097  /** Constructs a PneumaticHub with the default id (1). */
098  public PneumaticHub() {
099    this(SensorUtil.getDefaultREVPHModule());
100  }
101
102  /**
103   * Constructs a PneumaticHub.
104   *
105   * @param module module number to construct
106   */
107  public PneumaticHub(int module) {
108    m_dataStore = getForModule(module);
109    m_handle = m_dataStore.m_handle;
110  }
111
112  @Override
113  public void close() {
114    freeModule(m_dataStore);
115  }
116
117  @Override
118  public boolean getCompressor() {
119    return REVPHJNI.getCompressor(m_handle);
120  }
121
122  @Override
123  public CompressorConfigType getCompressorConfigType() {
124    return CompressorConfigType.fromValue(REVPHJNI.getCompressorConfig(m_handle));
125  }
126
127  @Override
128  public boolean getPressureSwitch() {
129    return REVPHJNI.getPressureSwitch(m_handle);
130  }
131
132  @Override
133  public double getCompressorCurrent() {
134    return REVPHJNI.getCompressorCurrent(m_handle);
135  }
136
137  @Override
138  public void setSolenoids(int mask, int values) {
139    REVPHJNI.setSolenoids(m_handle, mask, values);
140  }
141
142  @Override
143  public int getSolenoids() {
144    return REVPHJNI.getSolenoids(m_handle);
145  }
146
147  @Override
148  public int getModuleNumber() {
149    return m_dataStore.m_module;
150  }
151
152  @Override
153  public void fireOneShot(int index) {
154    REVPHJNI.fireOneShot(m_handle, index, m_dataStore.m_oneShotDurMs[index]);
155  }
156
157  @Override
158  public void setOneShotDuration(int index, int durMs) {
159    m_dataStore.m_oneShotDurMs[index] = durMs;
160  }
161
162  @Override
163  public boolean checkSolenoidChannel(int channel) {
164    return REVPHJNI.checkSolenoidChannel(channel);
165  }
166
167  @Override
168  public int checkAndReserveSolenoids(int mask) {
169    synchronized (m_dataStore.m_reserveLock) {
170      if ((m_dataStore.m_reservedMask & mask) != 0) {
171        return m_dataStore.m_reservedMask & mask;
172      }
173      m_dataStore.m_reservedMask |= mask;
174      return 0;
175    }
176  }
177
178  @Override
179  public void unreserveSolenoids(int mask) {
180    synchronized (m_dataStore.m_reserveLock) {
181      m_dataStore.m_reservedMask &= ~mask;
182    }
183  }
184
185  @Override
186  public Solenoid makeSolenoid(int channel) {
187    return new Solenoid(m_dataStore.m_module, PneumaticsModuleType.REVPH, channel);
188  }
189
190  @Override
191  public DoubleSolenoid makeDoubleSolenoid(int forwardChannel, int reverseChannel) {
192    return new DoubleSolenoid(
193        m_dataStore.m_module, PneumaticsModuleType.REVPH, forwardChannel, reverseChannel);
194  }
195
196  @Override
197  public Compressor makeCompressor() {
198    return new Compressor(m_dataStore.m_module, PneumaticsModuleType.REVPH);
199  }
200
201  @Override
202  public boolean reserveCompressor() {
203    synchronized (m_dataStore.m_reserveLock) {
204      if (m_dataStore.m_compressorReserved) {
205        return false;
206      }
207      m_dataStore.m_compressorReserved = true;
208      return true;
209    }
210  }
211
212  @Override
213  public void unreserveCompressor() {
214    synchronized (m_dataStore.m_reserveLock) {
215      m_dataStore.m_compressorReserved = false;
216    }
217  }
218
219  @Override
220  public int getSolenoidDisabledList() {
221    int raw = REVPHJNI.getStickyFaultsNative(m_handle);
222    return raw & 0xFFFF;
223  }
224
225  @Override
226  public void disableCompressor() {
227    REVPHJNI.setClosedLoopControlDisabled(m_handle);
228  }
229
230  @Override
231  public void enableCompressorDigital() {
232    REVPHJNI.setClosedLoopControlDigital(m_handle);
233  }
234
235  @Override
236  public void enableCompressorAnalog(double minPressure, double maxPressure) {
237    if (minPressure >= maxPressure) {
238      throw new IllegalArgumentException("maxPressure must be greater than minPressure");
239    }
240    if (minPressure < 0 || minPressure > 120) {
241      throw new IllegalArgumentException(
242          "minPressure must be between 0 and 120 PSI, got " + minPressure);
243    }
244    if (maxPressure < 0 || maxPressure > 120) {
245      throw new IllegalArgumentException(
246          "maxPressure must be between 0 and 120 PSI, got " + maxPressure);
247    }
248    double minAnalogVoltage = psiToVolts(minPressure, 5);
249    double maxAnalogVoltage = psiToVolts(maxPressure, 5);
250    REVPHJNI.setClosedLoopControlAnalog(m_handle, minAnalogVoltage, maxAnalogVoltage);
251  }
252
253  @Override
254  public void enableCompressorHybrid(double minPressure, double maxPressure) {
255    if (minPressure >= maxPressure) {
256      throw new IllegalArgumentException("maxPressure must be greater than minPressure");
257    }
258    if (minPressure < 0 || minPressure > 120) {
259      throw new IllegalArgumentException(
260          "minPressure must be between 0 and 120 PSI, got " + minPressure);
261    }
262    if (maxPressure < 0 || maxPressure > 120) {
263      throw new IllegalArgumentException(
264          "maxPressure must be between 0 and 120 PSI, got " + maxPressure);
265    }
266    double minAnalogVoltage = psiToVolts(minPressure, 5);
267    double maxAnalogVoltage = psiToVolts(maxPressure, 5);
268    REVPHJNI.setClosedLoopControlHybrid(m_handle, minAnalogVoltage, maxAnalogVoltage);
269  }
270
271  @Override
272  public double getAnalogVoltage(int channel) {
273    return REVPHJNI.getAnalogVoltage(m_handle, channel);
274  }
275
276  @Override
277  public double getPressure(int channel) {
278    double sensorVoltage = REVPHJNI.getAnalogVoltage(m_handle, channel);
279    double supplyVoltage = REVPHJNI.get5VVoltage(m_handle);
280    return voltsToPsi(sensorVoltage, supplyVoltage);
281  }
282
283  void clearStickyFaults() {
284    REVPHJNI.clearStickyFaults(m_handle);
285  }
286
287  REVPHVersion getVersion() {
288    return REVPHJNI.getVersion(m_handle);
289  }
290
291  REVPHFaults getFaults() {
292    return REVPHJNI.getFaults(m_handle);
293  }
294
295  REVPHStickyFaults getStickyFaults() {
296    return REVPHJNI.getStickyFaults(m_handle);
297  }
298
299  double getInputVoltage() {
300    return REVPHJNI.getInputVoltage(m_handle);
301  }
302
303  double get5VRegulatedVoltage() {
304    return REVPHJNI.get5VVoltage(m_handle);
305  }
306
307  double getSolenoidsTotalCurrent() {
308    return REVPHJNI.getSolenoidCurrent(m_handle);
309  }
310
311  double getSolenoidsVoltage() {
312    return REVPHJNI.getSolenoidVoltage(m_handle);
313  }
314}