001 /*----------------------------------------------------------------------------*/
002 /* Copyright (c) FIRST 2008-2012. All Rights Reserved. */
003 /* Open Source Software - may be modified and shared by FRC teams. The code */
004 /* must be accompanied by the FIRST BSD license file in the root directory of */
005 /* the project. */
006 /*----------------------------------------------------------------------------*/
007
008 package edu.wpi.first.wpilibj;
009
010 import edu.wpi.first.wpilibj.communication.UsageReporting;
011 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
012 import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
013 import edu.wpi.first.wpilibj.parsing.ISensor;
014 import edu.wpi.first.wpilibj.tables.ITable;
015
016 /**
017 * Ultrasonic rangefinder class.
018 * The Ultrasonic rangefinder measures absolute distance based on the round-trip time
019 * of a ping generated by the controller. These sensors use two transducers, a speaker and
020 * a microphone both tuned to the ultrasonic range. A common ultrasonic sensor, the Daventech SRF04
021 * requires a short pulse to be generated on a digital channel. This causes the chirp to be
022 * emmitted. A second line becomes high as the ping is transmitted and goes low when
023 * the echo is received. The time that the line is high determines the round trip distance
024 * (time of flight).
025 */
026 public class Ultrasonic extends SensorBase implements PIDSource, ISensor, LiveWindowSendable {
027
028 /**
029 * The units to return when PIDGet is called
030 */
031 public static class Unit {
032
033 /**
034 * The integer value representing this enumeration
035 */
036 public final int value;
037 static final int kInches_val = 0;
038 static final int kMillimeters_val = 1;
039 /**
040 * Use inches for PIDGet
041 */
042 public static final Unit kInches = new Unit(kInches_val);
043 /**
044 * Use millimeters for PIDGet
045 */
046 public static final Unit kMillimeter = new Unit(kMillimeters_val);
047
048 private Unit(int value) {
049 this.value = value;
050 }
051 }
052 private static final double kPingTime = 10 * 1e-6; ///< Time (sec) for the ping trigger pulse.
053 private static final int kPriority = 90; ///< Priority that the ultrasonic round robin task runs.
054 private static final double kMaxUltrasonicTime = 0.1; ///< Max time (ms) between readings.
055 private static final double kSpeedOfSoundInchesPerSec = 1130.0 * 12.0;
056 private static Ultrasonic m_firstSensor = null; // head of the ultrasonic sensor list
057 private static boolean m_automaticEnabled = false; // automatic round robin mode
058 private DigitalInput m_echoChannel = null;
059 private DigitalOutput m_pingChannel = null;
060 private boolean m_allocatedChannels;
061 private boolean m_enabled = false;
062 private Counter m_counter = null;
063 private Ultrasonic m_nextSensor = null;
064 private static Thread m_task = null; // task doing the round-robin automatic sensing
065 private Unit m_units;
066 private static int m_instances = 0;
067
068 /**
069 * Background task that goes through the list of ultrasonic sensors and pings each one in turn. The counter
070 * is configured to read the timing of the returned echo pulse.
071 *
072 * DANGER WILL ROBINSON, DANGER WILL ROBINSON:
073 * This code runs as a task and assumes that none of the ultrasonic sensors will change while it's
074 * running. If one does, then this will certainly break. Make sure to disable automatic mode before changing
075 * anything with the sensors!!
076 */
077 private class UltrasonicChecker extends Thread {
078
079 public synchronized void run() {
080 Ultrasonic u = null;
081 while (m_automaticEnabled) {
082 if (u == null) {
083 u = m_firstSensor;
084 }
085 if (u == null) {
086 return;
087 }
088 if (u.isEnabled()) {
089 u.m_pingChannel.pulse(kPingTime); // do the ping
090 }
091 u = u.m_nextSensor;
092 Timer.delay(.1); // wait for ping to return
093 }
094 }
095 }
096
097 /**
098 * Initialize the Ultrasonic Sensor.
099 * This is the common code that initializes the ultrasonic sensor given that there
100 * are two digital I/O channels allocated. If the system was running in automatic mode (round robin)
101 * when the new sensor is added, it is stopped, the sensor is added, then automatic mode is
102 * restored.
103 */
104 private synchronized void initialize() {
105 if (m_task == null) {
106 m_task = new UltrasonicChecker();
107 }
108 boolean originalMode = m_automaticEnabled;
109 setAutomaticMode(false); // kill task when adding a new sensor
110 m_nextSensor = m_firstSensor;
111 m_firstSensor = this;
112
113 m_counter = new Counter(m_echoChannel); // set up counter for this sensor
114 m_counter.setMaxPeriod(1.0);
115 m_counter.setSemiPeriodMode(true);
116 m_counter.reset();
117 m_counter.start();
118 m_enabled = true; // make it available for round robin scheduling
119 setAutomaticMode(originalMode);
120
121 m_instances++;
122 UsageReporting.report(UsageReporting.kResourceType_Ultrasonic, m_instances);
123 LiveWindow.addSensor("Ultrasonic", m_echoChannel.getModuleForRouting(), m_echoChannel.getChannel(), this);
124 }
125
126 /**
127 * Create an instance of the Ultrasonic Sensor using the default module.
128 * This is designed to supchannel the Daventech SRF04 and Vex ultrasonic sensors. This
129 * constructor assumes that both digital I/O channels are in the default digital module.
130 * @param pingChannel The digital output channel that sends the pulse to initiate the sensor sending
131 * the ping.
132 * @param echoChannel The digital input channel that receives the echo. The length of time that the
133 * echo is high represents the round trip time of the ping, and the distance.
134 * @param units The units returned in either kInches or kMilliMeters
135 */
136 public Ultrasonic(final int pingChannel, final int echoChannel, Unit units) {
137 m_pingChannel = new DigitalOutput(pingChannel);
138 m_echoChannel = new DigitalInput(echoChannel);
139 m_allocatedChannels = true;
140 m_units = units;
141 initialize();
142 }
143
144 /**
145 * Create an instance of the Ultrasonic Sensor using the default module.
146 * This is designed to supchannel the Daventech SRF04 and Vex ultrasonic sensors. This
147 * constructor assumes that both digital I/O channels are in the default digital module.
148 * Default unit is inches.
149 * @param pingChannel The digital output channel that sends the pulse to initiate the sensor sending
150 * the ping.
151 * @param echoChannel The digital input channel that receives the echo. The length of time that the
152 * echo is high represents the round trip time of the ping, and the distance.
153 */
154 public Ultrasonic(final int pingChannel, final int echoChannel) {
155 this(pingChannel, echoChannel, Unit.kInches);
156 }
157
158 /**
159 * Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo channel and a DigitalOutput
160 * for the ping channel.
161 * @param pingChannel The digital output object that starts the sensor doing a ping. Requires a 10uS pulse to start.
162 * @param echoChannel The digital input object that times the return pulse to determine the range.
163 * @param units The units returned in either kInches or kMilliMeters
164 */
165 public Ultrasonic(DigitalOutput pingChannel, DigitalInput echoChannel, Unit units) {
166 if (pingChannel == null || echoChannel == null) {
167 throw new NullPointerException("Null Channel Provided");
168 }
169 m_allocatedChannels = false;
170 m_pingChannel = pingChannel;
171 m_echoChannel = echoChannel;
172 m_units = units;
173 initialize();
174 }
175
176 /**
177 * Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo channel and a DigitalOutput
178 * for the ping channel.
179 * Default unit is inches.
180 * @param pingChannel The digital output object that starts the sensor doing a ping. Requires a 10uS pulse to start.
181 * @param echoChannel The digital input object that times the return pulse to determine the range.
182 */
183 public Ultrasonic(DigitalOutput pingChannel, DigitalInput echoChannel) {
184 this(pingChannel,echoChannel,Unit.kInches);
185 }
186
187
188 /**
189 * Create an instance of the Ultrasonic sensor using specified modules.
190 * This is designed to supchannel the Daventech SRF04 and Vex ultrasonic sensors. This
191 * constructors takes the channel and module slot for each of the required digital I/O channels.
192 * @param pingSlot The digital module that the pingChannel is in.
193 * @param pingChannel The digital output channel that sends the pulse to initiate the sensor
194 * sending the ping.
195 * @param echoSlot The digital module that the echoChannel is in.
196 * @param echoChannel The digital input channel that receives the echo. The length of time
197 * that the echo is high represents the round trip time of the ping, and the distance.
198 * @param units The units returned in either kInches or kMilliMeters
199 */
200 public Ultrasonic(final int pingSlot, final int pingChannel,
201 final int echoSlot, final int echoChannel, Unit units) {
202 m_pingChannel = new DigitalOutput(pingSlot, pingChannel);
203 m_echoChannel = new DigitalInput(echoSlot, echoChannel);
204 m_allocatedChannels = true;
205 m_units = units;
206 initialize();
207 }
208
209 /**
210 * Create an instance of the Ultrasonic sensor using specified modules.
211 * This is designed to supchannel the Daventech SRF04 and Vex ultrasonic sensors. This
212 * constructors takes the channel and module slot for each of the required digital I/O channels.
213 * Defualt unit is inches.
214 * @param pingSlot The digital module that the pingChannel is in.
215 * @param pingChannel The digital output channel that sends the pulse to initiate the sensor
216 * sending the ping.
217 * @param echoSlot The digital module that the echoChannel is in.
218 * @param echoChannel The digital input channel that receives the echo. The length of time
219 * that the echo is high represents the round trip time of the ping, and the distance.
220 */
221 public Ultrasonic(final int pingSlot, final int pingChannel,
222 final int echoSlot, final int echoChannel) {
223 this(pingSlot, pingChannel, echoSlot, echoChannel, Unit.kInches);
224 }
225
226 /**
227 * Destructor for the ultrasonic sensor.
228 * Delete the instance of the ultrasonic sensor by freeing the allocated digital channels.
229 * If the system was in automatic mode (round robin), then it is stopped, then started again
230 * after this sensor is removed (provided this wasn't the last sensor).
231 */
232 public synchronized void free() {
233 boolean wasAutomaticMode = m_automaticEnabled;
234 setAutomaticMode(false);
235 if (m_allocatedChannels) {
236 if (m_pingChannel != null) {
237 m_pingChannel.free();
238 }
239 if (m_echoChannel != null) {
240 m_echoChannel.free();
241 }
242 }
243
244 if (m_counter != null) {
245 m_counter.free();
246 m_counter = null;
247 }
248
249 m_pingChannel = null;
250 m_echoChannel = null;
251
252 if (this == m_firstSensor) {
253 m_firstSensor = m_nextSensor;
254 if (m_firstSensor == null) {
255 setAutomaticMode(false);
256 }
257 } else {
258 for (Ultrasonic s = m_firstSensor; s != null; s = s.m_nextSensor) {
259 if (this == s.m_nextSensor) {
260 s.m_nextSensor = s.m_nextSensor.m_nextSensor;
261 break;
262 }
263 }
264 }
265 if (m_firstSensor != null && wasAutomaticMode) {
266 setAutomaticMode(true);
267 }
268 }
269
270 /**
271 * Turn Automatic mode on/off.
272 * When in Automatic mode, all sensors will fire in round robin, waiting a set
273 * time between each sensor.
274 * @param enabling Set to true if round robin scheduling should start for all the ultrasonic sensors. This
275 * scheduling method assures that the sensors are non-interfering because no two sensors fire at the same time.
276 * If another scheduling algorithm is preffered, it can be implemented by pinging the sensors manually and waiting
277 * for the results to come back.
278 */
279 public void setAutomaticMode(boolean enabling) {
280 if (enabling == m_automaticEnabled) {
281 return; // ignore the case of no change
282 }
283 m_automaticEnabled = enabling;
284
285 if (enabling) {
286 // enabling automatic mode.
287 // Clear all the counters so no data is valid
288 for (Ultrasonic u = m_firstSensor; u != null; u = u.m_nextSensor) {
289 u.m_counter.reset();
290 }
291 // Start round robin task
292 m_task.start();
293 } else {
294 // disabling automatic mode. Wait for background task to stop running.
295 while (m_task.isAlive()) {
296 Timer.delay(.15); // just a little longer than the ping time for round-robin to stop
297 }
298 // clear all the counters (data now invalid) since automatic mode is stopped
299 for (Ultrasonic u = m_firstSensor; u != null; u = u.m_nextSensor) {
300 u.m_counter.reset();
301 }
302 }
303 }
304
305 /**
306 * Single ping to ultrasonic sensor.
307 * Send out a single ping to the ultrasonic sensor. This only works if automatic (round robin)
308 * mode is disabled. A single ping is sent out, and the counter should count the semi-period
309 * when it comes in. The counter is reset to make the current value invalid.
310 */
311 public void ping() {
312 setAutomaticMode(false); // turn off automatic round robin if pinging single sensor
313 m_counter.reset(); // reset the counter to zero (invalid data now)
314 m_pingChannel.pulse(kPingTime); // do the ping to start getting a single range
315 }
316
317 /**
318 * Check if there is a valid range measurement.
319 * The ranges are accumulated in a counter that will increment on each edge of the echo (return)
320 * signal. If the count is not at least 2, then the range has not yet been measured, and is invalid.
321 * @return true if the range is valid
322 */
323 public boolean isRangeValid() {
324 return m_counter.get() > 1;
325 }
326
327 /**
328 * Get the range in inches from the ultrasonic sensor.
329 * @return double Range in inches of the target returned from the ultrasonic sensor. If there is
330 * no valid value yet, i.e. at least one measurement hasn't completed, then return 0.
331 */
332 public double getRangeInches() {
333 if (isRangeValid()) {
334 return m_counter.getPeriod() * kSpeedOfSoundInchesPerSec / 2.0;
335 } else {
336 return 0;
337 }
338 }
339
340 /**
341 * Get the range in millimeters from the ultrasonic sensor.
342 * @return double Range in millimeters of the target returned by the ultrasonic sensor.
343 * If there is no valid value yet, i.e. at least one measurement hasn't complted, then return 0.
344 */
345 public double getRangeMM() {
346 return getRangeInches() * 25.4;
347 }
348
349 /**
350 * Get the range in the current DistanceUnit for the PIDSource base object.
351 *
352 * @return The range in DistanceUnit
353 */
354 public double pidGet() {
355 switch (m_units.value) {
356 case Unit.kInches_val:
357 return getRangeInches();
358 case Unit.kMillimeters_val:
359 return getRangeMM();
360 default:
361 return 0.0;
362 }
363 }
364
365 /**
366 * Set the current DistanceUnit that should be used for the PIDSource base object.
367 *
368 * @param units The DistanceUnit that should be used.
369 */
370 public void setDistanceUnits(Unit units) {
371 m_units = units;
372 }
373
374 /**
375 * Get the current DistanceUnit that is used for the PIDSource base object.
376 *
377 * @return The type of DistanceUnit that is being used.
378 */
379 public Unit getDistanceUnits() {
380 return m_units;
381 }
382
383 /**
384 * Is the ultrasonic enabled
385 * @return true if the ultrasonic is enabled
386 */
387 public boolean isEnabled() {
388 return m_enabled;
389 }
390
391 /**
392 * Set if the ultrasonic is enabled
393 * @param enable set to true to enable the ultrasonic
394 */
395 public void setEnabled(boolean enable) {
396 m_enabled = enable;
397 }
398
399 /*
400 * Live Window code, only does anything if live window is activated.
401 */
402 public String getSmartDashboardType(){
403 return "Ultrasonic";
404 }
405 private ITable m_table;
406
407 /**
408 * {@inheritDoc}
409 */
410 public void initTable(ITable subtable) {
411 m_table = subtable;
412 updateTable();
413 }
414
415 /**
416 * {@inheritDoc}
417 */
418 public ITable getTable(){
419 return m_table;
420 }
421
422 /**
423 * {@inheritDoc}
424 */
425 public void updateTable() {
426 if (m_table != null) {
427 m_table.putNumber("Value", getRangeInches());
428 }
429 }
430
431 /**
432 * {@inheritDoc}
433 */
434 public void startLiveWindowMode() {}
435
436 /**
437 * {@inheritDoc}
438 */
439 public void stopLiveWindowMode() {}
440 }