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 static java.util.Objects.requireNonNull;
008
009import edu.wpi.first.hal.FRCNetComm.tResourceType;
010import edu.wpi.first.hal.HAL;
011import edu.wpi.first.hal.SimBoolean;
012import edu.wpi.first.hal.SimDevice;
013import edu.wpi.first.hal.SimDevice.Direction;
014import edu.wpi.first.hal.SimDouble;
015import edu.wpi.first.util.sendable.Sendable;
016import edu.wpi.first.util.sendable.SendableBuilder;
017import edu.wpi.first.util.sendable.SendableRegistry;
018import java.util.ArrayList;
019import java.util.List;
020
021/**
022 * Ultrasonic rangefinder class. The Ultrasonic rangefinder measures absolute distance based on the
023 * round-trip time of a ping generated by the controller. These sensors use two transducers, a
024 * speaker and a microphone both tuned to the ultrasonic range. A common ultrasonic sensor, the
025 * Daventech SRF04 requires a short pulse to be generated on a digital channel. This causes the
026 * chirp to be emitted. A second line becomes high as the ping is transmitted and goes low when the
027 * echo is received. The time that the line is high determines the round trip distance (time of
028 * flight).
029 */
030public class Ultrasonic implements Sendable, AutoCloseable {
031  // Time (sec) for the ping trigger pulse.
032  private static final double kPingTime = 10 * 1e-6;
033  private static final double kSpeedOfSoundInchesPerSec = 1130.0 * 12.0;
034  // ultrasonic sensor list
035  private static final List<Ultrasonic> m_sensors = new ArrayList<>();
036  // automatic round robin mode
037  private static volatile boolean m_automaticEnabled;
038  private DigitalInput m_echoChannel;
039  private DigitalOutput m_pingChannel;
040  private final boolean m_allocatedChannels;
041  private boolean m_enabled;
042  private Counter m_counter;
043  // task doing the round-robin automatic sensing
044  private static Thread m_task;
045  private static int m_instances;
046
047  private SimDevice m_simDevice;
048  private SimBoolean m_simRangeValid;
049  private SimDouble m_simRange;
050
051  /**
052   * Background task that goes through the list of ultrasonic sensors and pings each one in turn.
053   * The counter is configured to read the timing of the returned echo pulse.
054   *
055   * <p><b>DANGER WILL ROBINSON, DANGER WILL ROBINSON:</b> This code runs as a task and assumes that
056   * none of the ultrasonic sensors will change while it's running. If one does, then this will
057   * certainly break. Make sure to disable automatic mode before changing anything with the
058   * sensors!!
059   */
060  private static class UltrasonicChecker extends Thread {
061    @Override
062    public synchronized void run() {
063      while (m_automaticEnabled) {
064        for (Ultrasonic sensor : m_sensors) {
065          if (!m_automaticEnabled) {
066            break;
067          }
068
069          if (sensor.isEnabled()) {
070            sensor.m_pingChannel.pulse(kPingTime); // do the ping
071          }
072
073          Timer.delay(0.1); // wait for ping to return
074        }
075      }
076    }
077  }
078
079  /**
080   * Initialize the Ultrasonic Sensor. This is the common code that initializes the ultrasonic
081   * sensor given that there are two digital I/O channels allocated. If the system was running in
082   * automatic mode (round robin) when the new sensor is added, it is stopped, the sensor is added,
083   * then automatic mode is restored.
084   */
085  private synchronized void initialize() {
086    m_simDevice = SimDevice.create("Ultrasonic", m_echoChannel.getChannel());
087    if (m_simDevice != null) {
088      m_simRangeValid = m_simDevice.createBoolean("Range Valid", Direction.kInput, true);
089      m_simRange = m_simDevice.createDouble("Range (in)", Direction.kInput, 0.0);
090      m_pingChannel.setSimDevice(m_simDevice);
091      m_echoChannel.setSimDevice(m_simDevice);
092    }
093    if (m_task == null) {
094      m_task = new UltrasonicChecker();
095    }
096    final boolean originalMode = m_automaticEnabled;
097    setAutomaticMode(false); // kill task when adding a new sensor
098    m_sensors.add(this);
099
100    m_counter = new Counter(m_echoChannel); // set up counter for this
101    SendableRegistry.addChild(this, m_counter);
102    // sensor
103    m_counter.setMaxPeriod(1.0);
104    m_counter.setSemiPeriodMode(true);
105    m_counter.reset();
106    m_enabled = true; // make it available for round robin scheduling
107    setAutomaticMode(originalMode);
108
109    m_instances++;
110    HAL.report(tResourceType.kResourceType_Ultrasonic, m_instances);
111    SendableRegistry.addLW(this, "Ultrasonic", m_echoChannel.getChannel());
112  }
113
114  public int getEchoChannel() {
115    return m_echoChannel.getChannel();
116  }
117
118  /**
119   * Create an instance of the Ultrasonic Sensor. This is designed to supchannel the Daventech SRF04
120   * and Vex ultrasonic sensors.
121   *
122   * @param pingChannel The digital output channel that sends the pulse to initiate the sensor
123   *     sending the ping.
124   * @param echoChannel The digital input channel that receives the echo. The length of time that
125   *     the echo is high represents the round trip time of the ping, and the distance.
126   */
127  public Ultrasonic(final int pingChannel, final int echoChannel) {
128    m_pingChannel = new DigitalOutput(pingChannel);
129    m_echoChannel = new DigitalInput(echoChannel);
130    SendableRegistry.addChild(this, m_pingChannel);
131    SendableRegistry.addChild(this, m_echoChannel);
132    m_allocatedChannels = true;
133    initialize();
134  }
135
136  /**
137   * Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo channel and a
138   * DigitalOutput for the ping channel.
139   *
140   * @param pingChannel The digital output object that starts the sensor doing a ping. Requires a
141   *     10uS pulse to start.
142   * @param echoChannel The digital input object that times the return pulse to determine the range.
143   */
144  public Ultrasonic(DigitalOutput pingChannel, DigitalInput echoChannel) {
145    requireNonNull(pingChannel, "Provided ping channel was null");
146    requireNonNull(echoChannel, "Provided echo channel was null");
147
148    m_allocatedChannels = false;
149    m_pingChannel = pingChannel;
150    m_echoChannel = echoChannel;
151    initialize();
152  }
153
154  /**
155   * Destructor for the ultrasonic sensor. Delete the instance of the ultrasonic sensor by freeing
156   * the allocated digital channels. If the system was in automatic mode (round robin), then it is
157   * stopped, then started again after this sensor is removed (provided this wasn't the last
158   * sensor).
159   */
160  @Override
161  public synchronized void close() {
162    SendableRegistry.remove(this);
163    final boolean wasAutomaticMode = m_automaticEnabled;
164    setAutomaticMode(false);
165    if (m_allocatedChannels) {
166      if (m_pingChannel != null) {
167        m_pingChannel.close();
168      }
169      if (m_echoChannel != null) {
170        m_echoChannel.close();
171      }
172    }
173
174    if (m_counter != null) {
175      m_counter.close();
176      m_counter = null;
177    }
178
179    m_pingChannel = null;
180    m_echoChannel = null;
181    synchronized (m_sensors) {
182      m_sensors.remove(this);
183    }
184    if (!m_sensors.isEmpty() && wasAutomaticMode) {
185      setAutomaticMode(true);
186    }
187
188    if (m_simDevice != null) {
189      m_simDevice.close();
190      m_simDevice = null;
191    }
192  }
193
194  /**
195   * Turn Automatic mode on/off for all sensors.
196   *
197   * <p>When in Automatic mode, all sensors will fire in round robin, waiting a set time between
198   * each sensor.
199   *
200   * @param enabling Set to true if round robin scheduling should start for all the ultrasonic
201   *     sensors. This scheduling method assures that the sensors are non-interfering because no two
202   *     sensors fire at the same time. If another scheduling algorithm is preferred, it can be
203   *     implemented by pinging the sensors manually and waiting for the results to come back.
204   */
205  public static void setAutomaticMode(boolean enabling) {
206    if (enabling == m_automaticEnabled) {
207      return; // ignore the case of no change
208    }
209    m_automaticEnabled = enabling;
210
211    if (enabling) {
212      /* Clear all the counters so no data is valid. No synchronization is
213       * needed because the background task is stopped.
214       */
215      for (Ultrasonic u : m_sensors) {
216        u.m_counter.reset();
217      }
218
219      // Start round robin task
220      m_task.start();
221    } else {
222      // Wait for background task to stop running
223      try {
224        m_task.join();
225      } catch (InterruptedException ex) {
226        Thread.currentThread().interrupt();
227        ex.printStackTrace();
228      }
229
230      /* Clear all the counters (data now invalid) since automatic mode is
231       * disabled. No synchronization is needed because the background task is
232       * stopped.
233       */
234      for (Ultrasonic u : m_sensors) {
235        u.m_counter.reset();
236      }
237    }
238  }
239
240  /**
241   * Single ping to ultrasonic sensor. Send out a single ping to the ultrasonic sensor. This only
242   * works if automatic (round robin) mode is disabled. A single ping is sent out, and the counter
243   * should count the semi-period when it comes in. The counter is reset to make the current value
244   * invalid.
245   */
246  public void ping() {
247    setAutomaticMode(false); // turn off automatic round robin if pinging
248    // single sensor
249    m_counter.reset(); // reset the counter to zero (invalid data now)
250    // do the ping to start getting a single range
251    m_pingChannel.pulse(kPingTime);
252  }
253
254  /**
255   * Check if there is a valid range measurement. The ranges are accumulated in a counter that will
256   * increment on each edge of the echo (return) signal. If the count is not at least 2, then the
257   * range has not yet been measured, and is invalid.
258   *
259   * @return true if the range is valid
260   */
261  public boolean isRangeValid() {
262    if (m_simRangeValid != null) {
263      return m_simRangeValid.get();
264    }
265    return m_counter.get() > 1;
266  }
267
268  /**
269   * Get the range in inches from the ultrasonic sensor. If there is no valid value yet, i.e. at
270   * least one measurement hasn't completed, then return 0.
271   *
272   * @return double Range in inches of the target returned from the ultrasonic sensor.
273   */
274  public double getRangeInches() {
275    if (isRangeValid()) {
276      if (m_simRange != null) {
277        return m_simRange.get();
278      }
279      return m_counter.getPeriod() * kSpeedOfSoundInchesPerSec / 2.0;
280    } else {
281      return 0;
282    }
283  }
284
285  /**
286   * Get the range in millimeters from the ultrasonic sensor. If there is no valid value yet, i.e.
287   * at least one measurement hasn't completed, then return 0.
288   *
289   * @return double Range in millimeters of the target returned by the ultrasonic sensor.
290   */
291  public double getRangeMM() {
292    return getRangeInches() * 25.4;
293  }
294
295  /**
296   * Is the ultrasonic enabled.
297   *
298   * @return true if the ultrasonic is enabled
299   */
300  public boolean isEnabled() {
301    return m_enabled;
302  }
303
304  /**
305   * Set if the ultrasonic is enabled.
306   *
307   * @param enable set to true to enable the ultrasonic
308   */
309  public void setEnabled(boolean enable) {
310    m_enabled = enable;
311  }
312
313  @Override
314  public void initSendable(SendableBuilder builder) {
315    builder.setSmartDashboardType("Ultrasonic");
316    builder.addDoubleProperty("Value", this::getRangeInches, null);
317  }
318}