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 edu.wpi.first.util.ErrorMessages.requireNonNullParam; 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 final boolean originalMode = m_automaticEnabled; 094 setAutomaticMode(false); // kill task when adding a new sensor 095 m_sensors.add(this); 096 097 m_counter = new Counter(m_echoChannel); // set up counter for this 098 SendableRegistry.addChild(this, m_counter); 099 // sensor 100 m_counter.setMaxPeriod(1.0); 101 m_counter.setSemiPeriodMode(true); 102 m_counter.reset(); 103 m_enabled = true; // make it available for round-robin scheduling 104 setAutomaticMode(originalMode); 105 106 m_instances++; 107 HAL.report(tResourceType.kResourceType_Ultrasonic, m_instances); 108 SendableRegistry.addLW(this, "Ultrasonic", m_echoChannel.getChannel()); 109 } 110 111 public int getEchoChannel() { 112 return m_echoChannel.getChannel(); 113 } 114 115 /** 116 * Create an instance of the Ultrasonic Sensor. This is designed to support the Daventech SRF04 117 * and Vex ultrasonic sensors. 118 * 119 * @param pingChannel The digital output channel that sends the pulse to initiate the sensor 120 * sending the ping. 121 * @param echoChannel The digital input channel that receives the echo. The length of time that 122 * the echo is high represents the round trip time of the ping, and the distance. 123 */ 124 @SuppressWarnings("this-escape") 125 public Ultrasonic(final int pingChannel, final int echoChannel) { 126 m_pingChannel = new DigitalOutput(pingChannel); 127 m_echoChannel = new DigitalInput(echoChannel); 128 SendableRegistry.addChild(this, m_pingChannel); 129 SendableRegistry.addChild(this, m_echoChannel); 130 m_allocatedChannels = true; 131 initialize(); 132 } 133 134 /** 135 * Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo channel and a 136 * DigitalOutput for the ping channel. 137 * 138 * @param pingChannel The digital output object that starts the sensor doing a ping. Requires a 139 * 10uS pulse to start. 140 * @param echoChannel The digital input object that times the return pulse to determine the range. 141 */ 142 @SuppressWarnings("this-escape") 143 public Ultrasonic(DigitalOutput pingChannel, DigitalInput echoChannel) { 144 requireNonNullParam(pingChannel, "pingChannel", "Ultrasonic"); 145 requireNonNullParam(echoChannel, "echoChannel", "Ultrasonic"); 146 147 m_allocatedChannels = false; 148 m_pingChannel = pingChannel; 149 m_echoChannel = echoChannel; 150 initialize(); 151 } 152 153 /** 154 * Destructor for the ultrasonic sensor. Delete the instance of the ultrasonic sensor by freeing 155 * the allocated digital channels. If the system was in automatic mode (round-robin), then it is 156 * stopped, then started again after this sensor is removed (provided this wasn't the last 157 * sensor). 158 */ 159 @Override 160 public synchronized void close() { 161 SendableRegistry.remove(this); 162 final boolean wasAutomaticMode = m_automaticEnabled; 163 setAutomaticMode(false); 164 if (m_allocatedChannels) { 165 if (m_pingChannel != null) { 166 m_pingChannel.close(); 167 } 168 if (m_echoChannel != null) { 169 m_echoChannel.close(); 170 } 171 } 172 173 if (m_counter != null) { 174 m_counter.close(); 175 m_counter = null; 176 } 177 178 m_pingChannel = null; 179 m_echoChannel = null; 180 synchronized (m_sensors) { 181 m_sensors.remove(this); 182 } 183 if (!m_sensors.isEmpty() && wasAutomaticMode) { 184 setAutomaticMode(true); 185 } 186 187 if (m_simDevice != null) { 188 m_simDevice.close(); 189 m_simDevice = null; 190 } 191 } 192 193 /** 194 * Turn Automatic mode on/off for all sensors. 195 * 196 * <p>When in Automatic mode, all sensors will fire in round-robin, waiting a set time between 197 * each sensor. 198 * 199 * @param enabling Set to true if round-robin scheduling should start for all the ultrasonic 200 * sensors. This scheduling method assures that the sensors are non-interfering because no two 201 * sensors fire at the same time. If another scheduling algorithm is preferred, it can be 202 * implemented by pinging the sensors manually and waiting for the results to come back. 203 */ 204 public static synchronized void setAutomaticMode(boolean enabling) { 205 if (enabling == m_automaticEnabled) { 206 return; // ignore the case of no change 207 } 208 m_automaticEnabled = enabling; 209 210 if (enabling) { 211 /* Clear all the counters so no data is valid. No synchronization is 212 * needed because the background task is stopped. 213 */ 214 for (Ultrasonic u : m_sensors) { 215 u.m_counter.reset(); 216 } 217 218 // Start round robin task 219 m_task = new UltrasonicChecker(); 220 m_task.start(); 221 } else { 222 if (m_task != null) { 223 // Wait for background task to stop running 224 try { 225 m_task.join(); 226 m_task = null; 227 } catch (InterruptedException ex) { 228 Thread.currentThread().interrupt(); 229 ex.printStackTrace(); 230 } 231 } 232 233 /* Clear all the counters (data now invalid) since automatic mode is 234 * disabled. No synchronization is needed because the background task is 235 * stopped. 236 */ 237 for (Ultrasonic u : m_sensors) { 238 u.m_counter.reset(); 239 } 240 } 241 } 242 243 /** 244 * Single ping to ultrasonic sensor. Send out a single ping to the ultrasonic sensor. This only 245 * works if automatic (round-robin) mode is disabled. A single ping is sent out, and the counter 246 * should count the semi-period when it comes in. The counter is reset to make the current value 247 * invalid. 248 */ 249 public void ping() { 250 setAutomaticMode(false); // turn off automatic round-robin if pinging 251 // single sensor 252 m_counter.reset(); // reset the counter to zero (invalid data now) 253 // do the ping to start getting a single range 254 m_pingChannel.pulse(kPingTime); 255 } 256 257 /** 258 * Check if there is a valid range measurement. The ranges are accumulated in a counter that will 259 * increment on each edge of the echo (return) signal. If the count is not at least 2, then the 260 * range has not yet been measured, and is invalid. 261 * 262 * @return true if the range is valid 263 */ 264 public boolean isRangeValid() { 265 if (m_simRangeValid != null) { 266 return m_simRangeValid.get(); 267 } 268 return m_counter.get() > 1; 269 } 270 271 /** 272 * Get the range in inches from the ultrasonic sensor. If there is no valid value yet, i.e. at 273 * least one measurement hasn't completed, then return 0. 274 * 275 * @return double Range in inches of the target returned from the ultrasonic sensor. 276 */ 277 public double getRangeInches() { 278 if (isRangeValid()) { 279 if (m_simRange != null) { 280 return m_simRange.get(); 281 } 282 return m_counter.getPeriod() * kSpeedOfSoundInchesPerSec / 2.0; 283 } else { 284 return 0; 285 } 286 } 287 288 /** 289 * Get the range in millimeters from the ultrasonic sensor. If there is no valid value yet, i.e. 290 * at least one measurement hasn't completed, then return 0. 291 * 292 * @return double Range in millimeters of the target returned by the ultrasonic sensor. 293 */ 294 public double getRangeMM() { 295 return getRangeInches() * 25.4; 296 } 297 298 /** 299 * Is the ultrasonic enabled. 300 * 301 * @return true if the ultrasonic is enabled 302 */ 303 public boolean isEnabled() { 304 return m_enabled; 305 } 306 307 /** 308 * Set if the ultrasonic is enabled. 309 * 310 * @param enable set to true to enable the ultrasonic 311 */ 312 public void setEnabled(boolean enable) { 313 m_enabled = enable; 314 } 315 316 @Override 317 public void initSendable(SendableBuilder builder) { 318 builder.setSmartDashboardType("Ultrasonic"); 319 builder.addDoubleProperty("Value", this::getRangeInches, null); 320 } 321}