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.HAL;
008import edu.wpi.first.hal.PortsJNI;
009import edu.wpi.first.hal.REVPHFaults;
010import edu.wpi.first.hal.REVPHJNI;
011import edu.wpi.first.hal.REVPHStickyFaults;
012import edu.wpi.first.hal.REVPHVersion;
013import java.util.HashMap;
014import java.util.Map;
015
016/** Module class for controlling a REV Robotics Pneumatic Hub. */
017public class PneumaticHub implements PneumaticsBase {
018  private static class DataStore implements AutoCloseable {
019    public final int m_module;
020    public final int m_handle;
021    private final int m_busId;
022    private int m_refCount;
023    private int m_reservedMask;
024    private boolean m_compressorReserved;
025    public final int[] m_oneShotDurMs = new int[PortsJNI.getNumREVPHChannels()];
026    private final Object m_reserveLock = new Object();
027
028    DataStore(int busId, int module) {
029      m_handle = REVPHJNI.initialize(busId, module);
030      m_module = module;
031      m_busId = busId;
032      m_handleMaps[busId].put(module, this);
033
034      final REVPHVersion version = REVPHJNI.getVersion(m_handle);
035      final String fwVersion =
036          version.firmwareMajor + "." + version.firmwareMinor + "." + version.firmwareFix;
037
038      // Check PH firmware version
039      if (version.firmwareMajor > 0 && version.firmwareMajor < 22) {
040        throw new IllegalStateException(
041            "The Pneumatic Hub has firmware version "
042                + fwVersion
043                + ", and must be updated to version 2022.0.0 or later "
044                + "using the REV Hardware Client.");
045      }
046    }
047
048    @Override
049    public void close() {
050      REVPHJNI.free(m_handle);
051      m_handleMaps[m_busId].remove(m_module);
052    }
053
054    public void addRef() {
055      m_refCount++;
056    }
057
058    public void removeRef() {
059      m_refCount--;
060      if (m_refCount == 0) {
061        this.close();
062      }
063    }
064  }
065
066  @SuppressWarnings({"unchecked", "rawtypes"})
067  private static final Map<Integer, DataStore>[] m_handleMaps =
068      (Map<Integer, DataStore>[]) new Map[PortsJNI.getNumCanBuses()];
069
070  private static final Object m_handleLock = new Object();
071
072  private static DataStore getForModule(int busId, int module) {
073    synchronized (m_handleLock) {
074      Map<Integer, DataStore> handleMap = m_handleMaps[busId];
075      if (handleMap == null) {
076        handleMap = new HashMap<>();
077        m_handleMaps[busId] = handleMap;
078      }
079
080      DataStore pcm = handleMap.get(module);
081      if (pcm == null) {
082        pcm = new DataStore(busId, module);
083      }
084      pcm.addRef();
085      return pcm;
086    }
087  }
088
089  private static void freeModule(DataStore store) {
090    synchronized (m_handleLock) {
091      store.removeRef();
092    }
093  }
094
095  /** Converts volts to PSI per the REV Analog Pressure Sensor datasheet. */
096  private static double voltsToPsi(double sensorVoltage, double supplyVoltage) {
097    return 250 * (sensorVoltage / supplyVoltage) - 25;
098  }
099
100  /** Converts PSI to volts per the REV Analog Pressure Sensor datasheet. */
101  private static double psiToVolts(double pressure, double supplyVoltage) {
102    return supplyVoltage * (0.004 * pressure + 0.1);
103  }
104
105  private final DataStore m_dataStore;
106  private final int m_handle;
107
108  /**
109   * Constructs a PneumaticHub with the default ID (1).
110   *
111   * @param busId The bus ID
112   */
113  public PneumaticHub(int busId) {
114    this(busId, SensorUtil.getDefaultREVPHModule());
115  }
116
117  /**
118   * Constructs a PneumaticHub.
119   *
120   * @param busId The bus ID
121   * @param module module number to construct
122   */
123  public PneumaticHub(int busId, int module) {
124    m_dataStore = getForModule(busId, module);
125    m_handle = m_dataStore.m_handle;
126  }
127
128  @Override
129  public void close() {
130    freeModule(m_dataStore);
131  }
132
133  @Override
134  public boolean getCompressor() {
135    return REVPHJNI.getCompressor(m_handle);
136  }
137
138  @Override
139  public CompressorConfigType getCompressorConfigType() {
140    return CompressorConfigType.fromValue(REVPHJNI.getCompressorConfig(m_handle));
141  }
142
143  @Override
144  public boolean getPressureSwitch() {
145    return REVPHJNI.getPressureSwitch(m_handle);
146  }
147
148  @Override
149  public double getCompressorCurrent() {
150    return REVPHJNI.getCompressorCurrent(m_handle);
151  }
152
153  @Override
154  public void setSolenoids(int mask, int values) {
155    REVPHJNI.setSolenoids(m_handle, mask, values);
156  }
157
158  @Override
159  public int getSolenoids() {
160    return REVPHJNI.getSolenoids(m_handle);
161  }
162
163  @Override
164  public int getModuleNumber() {
165    return m_dataStore.m_module;
166  }
167
168  @Override
169  public void fireOneShot(int index) {
170    REVPHJNI.fireOneShot(m_handle, index, m_dataStore.m_oneShotDurMs[index]);
171  }
172
173  @Override
174  public void setOneShotDuration(int index, int durMs) {
175    m_dataStore.m_oneShotDurMs[index] = durMs;
176  }
177
178  @Override
179  public boolean checkSolenoidChannel(int channel) {
180    return REVPHJNI.checkSolenoidChannel(channel);
181  }
182
183  @Override
184  public int checkAndReserveSolenoids(int mask) {
185    synchronized (m_dataStore.m_reserveLock) {
186      if ((m_dataStore.m_reservedMask & mask) != 0) {
187        return m_dataStore.m_reservedMask & mask;
188      }
189      m_dataStore.m_reservedMask |= mask;
190      return 0;
191    }
192  }
193
194  @Override
195  public void unreserveSolenoids(int mask) {
196    synchronized (m_dataStore.m_reserveLock) {
197      m_dataStore.m_reservedMask &= ~mask;
198    }
199  }
200
201  @Override
202  public Solenoid makeSolenoid(int channel) {
203    return new Solenoid(m_dataStore.m_module, PneumaticsModuleType.REVPH, channel);
204  }
205
206  @Override
207  public DoubleSolenoid makeDoubleSolenoid(int forwardChannel, int reverseChannel) {
208    return new DoubleSolenoid(
209        m_dataStore.m_module, PneumaticsModuleType.REVPH, forwardChannel, reverseChannel);
210  }
211
212  @Override
213  public Compressor makeCompressor() {
214    return new Compressor(m_dataStore.m_module, PneumaticsModuleType.REVPH);
215  }
216
217  @Override
218  public boolean reserveCompressor() {
219    synchronized (m_dataStore.m_reserveLock) {
220      if (m_dataStore.m_compressorReserved) {
221        return false;
222      }
223      m_dataStore.m_compressorReserved = true;
224      return true;
225    }
226  }
227
228  @Override
229  public void unreserveCompressor() {
230    synchronized (m_dataStore.m_reserveLock) {
231      m_dataStore.m_compressorReserved = false;
232    }
233  }
234
235  @Override
236  public int getSolenoidDisabledList() {
237    return REVPHJNI.getSolenoidDisabledList(m_handle);
238  }
239
240  /**
241   * Disables the compressor. The compressor will not turn on until {@link
242   * #enableCompressorDigital()}, {@link #enableCompressorAnalog(double, double)}, or {@link
243   * #enableCompressorHybrid(double, double)} are called.
244   */
245  @Override
246  public void disableCompressor() {
247    REVPHJNI.setClosedLoopControlDisabled(m_handle);
248  }
249
250  @Override
251  public void enableCompressorDigital() {
252    REVPHJNI.setClosedLoopControlDigital(m_handle);
253  }
254
255  /**
256   * Enables the compressor in analog mode. This mode uses an analog pressure sensor connected to
257   * analog channel 0 to cycle the compressor. The compressor will turn on when the pressure drops
258   * below {@code minPressure} and will turn off when the pressure reaches {@code maxPressure}.
259   *
260   * @param minPressure The minimum pressure in PSI. The compressor will turn on when the pressure
261   *     drops below this value. Range 0-120 PSI.
262   * @param maxPressure The maximum pressure in PSI. The compressor will turn off when the pressure
263   *     reaches this value. Range 0-120 PSI. Must be larger then minPressure.
264   */
265  @Override
266  public void enableCompressorAnalog(double minPressure, double maxPressure) {
267    if (minPressure >= maxPressure) {
268      throw new IllegalArgumentException("maxPressure must be greater than minPressure");
269    }
270    if (minPressure < 0 || minPressure > 120) {
271      throw new IllegalArgumentException(
272          "minPressure must be between 0 and 120 PSI, got " + minPressure);
273    }
274    if (maxPressure < 0 || maxPressure > 120) {
275      throw new IllegalArgumentException(
276          "maxPressure must be between 0 and 120 PSI, got " + maxPressure);
277    }
278
279    // Send the voltage as it would be if the 5V rail was at exactly 5V.
280    // The firmware will compensate for the real 5V rail voltage, which
281    // can fluctuate somewhat over time.
282    double minAnalogVoltage = psiToVolts(minPressure, 5);
283    double maxAnalogVoltage = psiToVolts(maxPressure, 5);
284    REVPHJNI.setClosedLoopControlAnalog(m_handle, minAnalogVoltage, maxAnalogVoltage);
285  }
286
287  /**
288   * Enables the compressor in hybrid mode. This mode uses both a digital pressure switch and an
289   * analog pressure sensor connected to analog channel 0 to cycle the compressor.
290   *
291   * <p>The compressor will turn on when <i>both</i>:
292   *
293   * <ul>
294   *   <li>The digital pressure switch indicates the system is not full AND
295   *   <li>The analog pressure sensor indicates that the pressure in the system is below the
296   *       specified minimum pressure.
297   * </ul>
298   *
299   * <p>The compressor will turn off when <i>either</i>:
300   *
301   * <ul>
302   *   <li>The digital pressure switch is disconnected or indicates that the system is full OR
303   *   <li>The pressure detected by the analog sensor is greater than the specified maximum
304   *       pressure.
305   * </ul>
306   *
307   * @param minPressure The minimum pressure in PSI. The compressor will turn on when the pressure
308   *     drops below this value and the pressure switch indicates that the system is not full. Range
309   *     0-120 PSI.
310   * @param maxPressure The maximum pressure in PSI. The compressor will turn off when the pressure
311   *     reaches this value or the pressure switch is disconnected or indicates that the system is
312   *     full. Range 0-120 PSI. Must be larger then minPressure.
313   */
314  @Override
315  public void enableCompressorHybrid(double minPressure, double maxPressure) {
316    if (minPressure >= maxPressure) {
317      throw new IllegalArgumentException("maxPressure must be greater than minPressure");
318    }
319    if (minPressure < 0 || minPressure > 120) {
320      throw new IllegalArgumentException(
321          "minPressure must be between 0 and 120 PSI, got " + minPressure);
322    }
323    if (maxPressure < 0 || maxPressure > 120) {
324      throw new IllegalArgumentException(
325          "maxPressure must be between 0 and 120 PSI, got " + maxPressure);
326    }
327
328    // Send the voltage as it would be if the 5V rail was at exactly 5V.
329    // The firmware will compensate for the real 5V rail voltage, which
330    // can fluctuate somewhat over time.
331    double minAnalogVoltage = psiToVolts(minPressure, 5);
332    double maxAnalogVoltage = psiToVolts(maxPressure, 5);
333    REVPHJNI.setClosedLoopControlHybrid(m_handle, minAnalogVoltage, maxAnalogVoltage);
334  }
335
336  /**
337   * Returns the raw voltage of the specified analog input channel.
338   *
339   * @param channel The analog input channel to read voltage from.
340   * @return The voltage of the specified analog input channel.
341   */
342  @Override
343  public double getAnalogVoltage(int channel) {
344    return REVPHJNI.getAnalogVoltage(m_handle, channel);
345  }
346
347  /**
348   * Returns the pressure read by an analog pressure sensor on the specified analog input channel.
349   *
350   * @param channel The analog input channel to read pressure from.
351   * @return The pressure read by an analog pressure sensor on the specified analog input channel.
352   */
353  @Override
354  public double getPressure(int channel) {
355    double sensorVoltage = REVPHJNI.getAnalogVoltage(m_handle, channel);
356    double supplyVoltage = REVPHJNI.get5VVoltage(m_handle);
357    return voltsToPsi(sensorVoltage, supplyVoltage);
358  }
359
360  /** Clears the sticky faults. */
361  public void clearStickyFaults() {
362    REVPHJNI.clearStickyFaults(m_handle);
363  }
364
365  /**
366   * Returns the hardware and firmware versions of this device.
367   *
368   * @return The hardware and firmware versions.
369   */
370  public REVPHVersion getVersion() {
371    return REVPHJNI.getVersion(m_handle);
372  }
373
374  /**
375   * Returns the faults currently active on this device.
376   *
377   * @return The faults.
378   */
379  public REVPHFaults getFaults() {
380    return REVPHJNI.getFaults(m_handle);
381  }
382
383  /**
384   * Returns the sticky faults currently active on this device.
385   *
386   * @return The sticky faults.
387   */
388  public REVPHStickyFaults getStickyFaults() {
389    return REVPHJNI.getStickyFaults(m_handle);
390  }
391
392  /**
393   * Returns the current input voltage for this device.
394   *
395   * @return The input voltage.
396   */
397  public double getInputVoltage() {
398    return REVPHJNI.getInputVoltage(m_handle);
399  }
400
401  /**
402   * Returns the current voltage of the regulated 5v supply.
403   *
404   * @return The current voltage of the 5v supply.
405   */
406  public double get5VRegulatedVoltage() {
407    return REVPHJNI.get5VVoltage(m_handle);
408  }
409
410  /**
411   * Returns the total current (in amps) drawn by all solenoids.
412   *
413   * @return Total current drawn by all solenoids in amps.
414   */
415  public double getSolenoidsTotalCurrent() {
416    return REVPHJNI.getSolenoidCurrent(m_handle);
417  }
418
419  /**
420   * Returns the current voltage of the solenoid power supply.
421   *
422   * @return The current voltage of the solenoid power supply.
423   */
424  public double getSolenoidsVoltage() {
425    return REVPHJNI.getSolenoidVoltage(m_handle);
426  }
427
428  @Override
429  public void reportUsage(String device, String data) {
430    HAL.reportUsage("PH[" + m_dataStore.m_module + "]/" + device, data);
431  }
432}