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.SimDevice;
009import edu.wpi.first.hal.SimDevice.Direction;
010import edu.wpi.first.hal.SimDouble;
011import edu.wpi.first.math.MathUtil;
012import edu.wpi.first.util.sendable.Sendable;
013import edu.wpi.first.util.sendable.SendableBuilder;
014import edu.wpi.first.util.sendable.SendableRegistry;
015
016/**
017 * SharpIR analog distance sensor class. These distance measuring sensors output an analog voltage
018 * corresponding to the detection distance.
019 *
020 * <p>Teams are advised that the case of these sensors are conductive and grounded, so they should
021 * not be mounted on a metallic surface on an FRC robot.
022 */
023@SuppressWarnings("MethodName")
024public class SharpIR implements Sendable, AutoCloseable {
025  private AnalogInput m_sensor;
026
027  private SimDevice m_simDevice;
028  private SimDouble m_simRange;
029
030  private final double m_A;
031  private final double m_B;
032  private final double m_min; // m
033  private final double m_max; // m
034
035  /**
036   * Sharp GP2Y0A02YK0F is an analog IR sensor capable of measuring distances from 20 cm to 150 cm.
037   *
038   * @param channel Analog input channel the sensor is connected to
039   * @return sensor object
040   */
041  public static SharpIR GP2Y0A02YK0F(int channel) {
042    return new SharpIR(channel, 62.28, -1.092, 0.2, 1.5);
043  }
044
045  /**
046   * Sharp GP2Y0A21YK0F is an analog IR sensor capable of measuring distances from 10 cm to 80 cm.
047   *
048   * @param channel Analog input channel the sensor is connected to
049   * @return sensor object
050   */
051  public static SharpIR GP2Y0A21YK0F(int channel) {
052    return new SharpIR(channel, 26.449, -1.226, 0.1, 0.8);
053  }
054
055  /**
056   * Sharp GP2Y0A41SK0F is an analog IR sensor capable of measuring distances from 4 cm to 30 cm.
057   *
058   * @param channel Analog input channel the sensor is connected to
059   * @return sensor object
060   */
061  public static SharpIR GP2Y0A41SK0F(int channel) {
062    return new SharpIR(channel, 12.354, -1.07, 0.04, 0.3);
063  }
064
065  /**
066   * Sharp GP2Y0A51SK0F is an analog IR sensor capable of measuring distances from 2 cm to 15 cm.
067   *
068   * @param channel Analog input channel the sensor is connected to
069   * @return sensor object
070   */
071  public static SharpIR GP2Y0A51SK0F(int channel) {
072    return new SharpIR(channel, 5.2819, -1.161, 0.02, 0.15);
073  }
074
075  /**
076   * Manually construct a SharpIR object. The distance is computed using this formula: A*v ^ B.
077   * Prefer to use one of the static factories to create this device instead.
078   *
079   * @param channel AnalogInput channel
080   * @param a Constant A
081   * @param b Constant B
082   * @param min Minimum distance to report in meters
083   * @param max Maximum distance to report in meters
084   */
085  @SuppressWarnings("this-escape")
086  public SharpIR(int channel, double a, double b, double min, double max) {
087    m_sensor = new AnalogInput(channel);
088
089    m_A = a;
090    m_B = b;
091    m_min = min;
092    m_max = max;
093
094    HAL.reportUsage("IO", channel, "SharpIR");
095    SendableRegistry.add(this, "SharpIR", channel);
096
097    m_simDevice = SimDevice.create("SharpIR", m_sensor.getChannel());
098    if (m_simDevice != null) {
099      m_simRange = m_simDevice.createDouble("Range (m)", Direction.kInput, 0.0);
100      m_sensor.setSimDevice(m_simDevice);
101    }
102  }
103
104  @Override
105  public void close() {
106    SendableRegistry.remove(this);
107    m_sensor.close();
108    m_sensor = null;
109
110    if (m_simDevice != null) {
111      m_simDevice.close();
112      m_simDevice = null;
113      m_simRange = null;
114    }
115  }
116
117  /**
118   * Get the analog input channel number.
119   *
120   * @return analog input channel
121   */
122  public int getChannel() {
123    return m_sensor.getChannel();
124  }
125
126  /**
127   * Get the range in meters from the distance sensor.
128   *
129   * @return range in meters of the target returned by the sensor
130   */
131  public double getRange() {
132    if (m_simRange != null) {
133      return MathUtil.clamp(m_simRange.get(), m_min, m_max);
134    } else {
135      // Don't allow zero/negative values
136      var v = Math.max(m_sensor.getVoltage(), 0.00001);
137
138      return MathUtil.clamp(m_A * Math.pow(v, m_B) * 1e-2, m_min, m_max);
139    }
140  }
141
142  @Override
143  public void initSendable(SendableBuilder builder) {
144    builder.setSmartDashboardType("Ultrasonic");
145    builder.addDoubleProperty("Value", this::getRange, null);
146  }
147}