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