Ultrasonic.java

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj;

import edu.wpi.first.wpilibj.communication.UsageReporting;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
import edu.wpi.first.wpilibj.parsing.ISensor;
import edu.wpi.first.wpilibj.tables.ITable;

/**
 * Ultrasonic rangefinder class.
 * The Ultrasonic rangefinder measures absolute distance based on the round-trip time
 * of a ping generated by the controller. These sensors use two transducers, a speaker and
 * a microphone both tuned to the ultrasonic range. A common ultrasonic sensor, the Daventech SRF04
 * requires a short pulse to be generated on a digital channel. This causes the chirp to be
 * emmitted. A second line becomes high as the ping is transmitted and goes low when
 * the echo is received. The time that the line is high determines the round trip distance
 * (time of flight).
 */
public class Ultrasonic extends SensorBase implements PIDSource, ISensor, LiveWindowSendable {

    /**
     * The units to return when PIDGet is called
     */
    public static class Unit {

        /**
         * The integer value representing this enumeration
         */
        public final int value;
        static final int kInches_val = 0;
        static final int kMillimeters_val = 1;
        /**
         * Use inches for PIDGet
         */
        public static final Unit kInches = new Unit(kInches_val);
        /**
         * Use millimeters for PIDGet
         */
        public static final Unit kMillimeter = new Unit(kMillimeters_val);

        private Unit(int value) {
            this.value = value;
        }
    }
    private static final double kPingTime = 10 * 1e-6;	///< Time (sec) for the ping trigger pulse.
    private static final int kPriority = 90;	///< Priority that the ultrasonic round robin task runs.
    private static final double kMaxUltrasonicTime = 0.1;	///< Max time (ms) between readings.
    private static final double kSpeedOfSoundInchesPerSec = 1130.0 * 12.0;
    private static Ultrasonic m_firstSensor = null; // head of the ultrasonic sensor list
    private static boolean m_automaticEnabled = false; // automatic round robin mode
    private DigitalInput m_echoChannel = null;
    private DigitalOutput m_pingChannel = null;
    private boolean m_allocatedChannels;
    private boolean m_enabled = false;
    private Counter m_counter = null;
    private Ultrasonic m_nextSensor = null;
    private static Thread m_task = null; // task doing the round-robin automatic sensing
    private Unit m_units;
    private static int m_instances = 0;

    /**
     * Background task that goes through the list of ultrasonic sensors and pings each one in turn. The counter
     * is configured to read the timing of the returned echo pulse.
     *
     * DANGER WILL ROBINSON, DANGER WILL ROBINSON:
     * This code runs as a task and assumes that none of the ultrasonic sensors will change while it's
     * running. If one does, then this will certainly break. Make sure to disable automatic mode before changing
     * anything with the sensors!!
     */
    private class UltrasonicChecker extends Thread {

        public synchronized void run() {
            Ultrasonic u = null;
            while (m_automaticEnabled) {
                if (u == null) {
                    u = m_firstSensor;
                }
                if (u == null) {
                    return;
                }
                if (u.isEnabled()) {
                    u.m_pingChannel.pulse(kPingTime);	// do the ping
                }
                u = u.m_nextSensor;
                Timer.delay(.1);							// wait for ping to return
            }
        }
    }

    /**
     * Initialize the Ultrasonic Sensor.
     * This is the common code that initializes the ultrasonic sensor given that there
     * are two digital I/O channels allocated. If the system was running in automatic mode (round robin)
     * when the new sensor is added, it is stopped, the sensor is added, then automatic mode is
     * restored.
     */
    private synchronized void initialize() {
        if (m_task == null) {
            m_task = new UltrasonicChecker();
        }
        boolean originalMode = m_automaticEnabled;
        setAutomaticMode(false); // kill task when adding a new sensor
        m_nextSensor = m_firstSensor;
        m_firstSensor = this;

        m_counter = new Counter(m_echoChannel); // set up counter for this sensor
        m_counter.setMaxPeriod(1.0);
        m_counter.setSemiPeriodMode(true);
        m_counter.reset();
        m_counter.start();
        m_enabled = true; // make it available for round robin scheduling
        setAutomaticMode(originalMode);

        m_instances++;
        UsageReporting.report(UsageReporting.kResourceType_Ultrasonic, m_instances);
        LiveWindow.addSensor("Ultrasonic", m_echoChannel.getModuleForRouting(), m_echoChannel.getChannel(), this);
    }

    /**
     * Create an instance of the Ultrasonic Sensor using the default module.
     * This is designed to supchannel the Daventech SRF04 and Vex ultrasonic sensors. This
     * constructor assumes that both digital I/O channels are in the default digital module.
     * @param pingChannel The digital output channel that sends the pulse to initiate the sensor sending
     * the ping.
     * @param echoChannel The digital input channel that receives the echo. The length of time that the
     * echo is high represents the round trip time of the ping, and the distance.
     * @param units The units returned in either kInches or kMilliMeters
     */
    public Ultrasonic(final int pingChannel, final int echoChannel, Unit units) {
        m_pingChannel = new DigitalOutput(pingChannel);
        m_echoChannel = new DigitalInput(echoChannel);
        m_allocatedChannels = true;
        m_units = units;
        initialize();
    }

    /**
     * Create an instance of the Ultrasonic Sensor using the default module.
     * This is designed to supchannel the Daventech SRF04 and Vex ultrasonic sensors. This
     * constructor assumes that both digital I/O channels are in the default digital module.
     * Default unit is inches.
     * @param pingChannel The digital output channel that sends the pulse to initiate the sensor sending
     * the ping.
     * @param echoChannel The digital input channel that receives the echo. The length of time that the
     * echo is high represents the round trip time of the ping, and the distance.
     */
    public Ultrasonic(final int pingChannel, final int echoChannel) {
        this(pingChannel, echoChannel, Unit.kInches);
    }

    /**
     * Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo channel and a DigitalOutput
     * for the ping channel.
     * @param pingChannel The digital output object that starts the sensor doing a ping. Requires a 10uS pulse to start.
     * @param echoChannel The digital input object that times the return pulse to determine the range.
     * @param units The units returned in either kInches or kMilliMeters
     */
    public Ultrasonic(DigitalOutput pingChannel, DigitalInput echoChannel, Unit units) {
        if (pingChannel == null || echoChannel == null) {
            throw new NullPointerException("Null Channel Provided");
        }
        m_allocatedChannels = false;
        m_pingChannel = pingChannel;
        m_echoChannel = echoChannel;
        m_units = units;
        initialize();
    }

    /**
     * Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo channel and a DigitalOutput
     * for the ping channel.
     * Default unit is inches.
     * @param pingChannel The digital output object that starts the sensor doing a ping. Requires a 10uS pulse to start.
     * @param echoChannel The digital input object that times the return pulse to determine the range.
     */
    public Ultrasonic(DigitalOutput pingChannel, DigitalInput echoChannel) {
        this(pingChannel,echoChannel,Unit.kInches);
    }


    /**
     * Create an instance of the Ultrasonic sensor using specified modules.
     * This is designed to supchannel the Daventech SRF04 and Vex ultrasonic sensors. This
     * constructors takes the channel and module slot for each of the required digital I/O channels.
     * @param pingSlot The digital module that the pingChannel is in.
     * @param pingChannel The digital output channel that sends the pulse to initiate the sensor
     * sending the ping.
     * @param echoSlot The digital module that the echoChannel is in.
     * @param echoChannel The digital input channel that receives the echo. The length of time
     * that the echo is high represents the round trip time of the ping, and the distance.
     * @param units The units returned in either kInches or kMilliMeters
     */
    public Ultrasonic(final int pingSlot, final int pingChannel,
            final int echoSlot, final int echoChannel, Unit units) {
        m_pingChannel = new DigitalOutput(pingSlot, pingChannel);
        m_echoChannel = new DigitalInput(echoSlot, echoChannel);
        m_allocatedChannels = true;
        m_units = units;
        initialize();
    }

    /**
     * Create an instance of the Ultrasonic sensor using specified modules.
     * This is designed to supchannel the Daventech SRF04 and Vex ultrasonic sensors. This
     * constructors takes the channel and module slot for each of the required digital I/O channels.
     * Defualt unit is inches.
     * @param pingSlot The digital module that the pingChannel is in.
     * @param pingChannel The digital output channel that sends the pulse to initiate the sensor
     * sending the ping.
     * @param echoSlot The digital module that the echoChannel is in.
     * @param echoChannel The digital input channel that receives the echo. The length of time
     * that the echo is high represents the round trip time of the ping, and the distance.
     */
    public Ultrasonic(final int pingSlot, final int pingChannel,
            final int echoSlot, final int echoChannel) {
        this(pingSlot, pingChannel, echoSlot, echoChannel, Unit.kInches);
    }
    
    /**
     * Destructor for the ultrasonic sensor.
     * Delete the instance of the ultrasonic sensor by freeing the allocated digital channels.
     * If the system was in automatic mode (round robin), then it is stopped, then started again
     * after this sensor is removed (provided this wasn't the last sensor).
     */
    public synchronized void free() {
        boolean wasAutomaticMode = m_automaticEnabled;
        setAutomaticMode(false);
        if (m_allocatedChannels) {
            if (m_pingChannel != null) {
                m_pingChannel.free();
            }
            if (m_echoChannel != null) {
                m_echoChannel.free();
            }
        }

        if (m_counter != null) {
            m_counter.free();
            m_counter = null;
        }

        m_pingChannel = null;
        m_echoChannel = null;

        if (this == m_firstSensor) {
            m_firstSensor = m_nextSensor;
            if (m_firstSensor == null) {
                setAutomaticMode(false);
            }
        } else {
            for (Ultrasonic s = m_firstSensor; s != null; s = s.m_nextSensor) {
                if (this == s.m_nextSensor) {
                    s.m_nextSensor = s.m_nextSensor.m_nextSensor;
                    break;
                }
            }
        }
        if (m_firstSensor != null && wasAutomaticMode) {
            setAutomaticMode(true);
        }
    }

    /**
     * Turn Automatic mode on/off.
     * When in Automatic mode, all sensors will fire in round robin, waiting a set
     * time between each sensor.
     * @param enabling Set to true if round robin scheduling should start for all the ultrasonic sensors. This
     * scheduling method assures that the sensors are non-interfering because no two sensors fire at the same time.
     * If another scheduling algorithm is preffered, it can be implemented by pinging the sensors manually and waiting
     * for the results to come back.
     */
    public void setAutomaticMode(boolean enabling) {
        if (enabling == m_automaticEnabled) {
            return; // ignore the case of no change
        }
        m_automaticEnabled = enabling;

        if (enabling) {
            // enabling automatic mode.
            // Clear all the counters so no data is valid
            for (Ultrasonic u = m_firstSensor; u != null; u = u.m_nextSensor) {
                u.m_counter.reset();
            }
            // Start round robin task
            m_task.start();
        } else {
            // disabling automatic mode. Wait for background task to stop running.
            while (m_task.isAlive()) {
                Timer.delay(.15);	// just a little longer than the ping time for round-robin to stop
            }
            // clear all the counters (data now invalid) since automatic mode is stopped
            for (Ultrasonic u = m_firstSensor; u != null; u = u.m_nextSensor) {
                u.m_counter.reset();
            }
        }
    }

    /**
     * Single ping to ultrasonic sensor.
     * Send out a single ping to the ultrasonic sensor. This only works if automatic (round robin)
     * mode is disabled. A single ping is sent out, and the counter should count the semi-period
     * when it comes in. The counter is reset to make the current value invalid.
     */
    public void ping() {
        setAutomaticMode(false); // turn off automatic round robin if pinging single sensor
        m_counter.reset(); // reset the counter to zero (invalid data now)
        m_pingChannel.pulse(kPingTime); // do the ping to start getting a single range
    }

    /**
     * Check if there is a valid range measurement.
     * The ranges are accumulated in a counter that will increment on each edge of the echo (return)
     * signal. If the count is not at least 2, then the range has not yet been measured, and is invalid.
     * @return true if the range is valid
     */
    public boolean isRangeValid() {
        return m_counter.get() > 1;
    }

    /**
     * Get the range in inches from the ultrasonic sensor.
     * @return double Range in inches of the target returned from the ultrasonic sensor. If there is
     * no valid value yet, i.e. at least one measurement hasn't completed, then return 0.
     */
    public double getRangeInches() {
        if (isRangeValid()) {
            return m_counter.getPeriod() * kSpeedOfSoundInchesPerSec / 2.0;
        } else {
            return 0;
        }
    }

    /**
     * Get the range in millimeters from the ultrasonic sensor.
     * @return double Range in millimeters of the target returned by the ultrasonic sensor.
     * If there is no valid value yet, i.e. at least one measurement hasn't complted, then return 0.
     */
    public double getRangeMM() {
        return getRangeInches() * 25.4;
    }

    /**
     * Get the range in the current DistanceUnit for the PIDSource base object.
     *
     * @return The range in DistanceUnit
     */
    public double pidGet() {
        switch (m_units.value) {
            case Unit.kInches_val:
                return getRangeInches();
            case Unit.kMillimeters_val:
                return getRangeMM();
            default:
                return 0.0;
        }
    }

    /**
     * Set the current DistanceUnit that should be used for the PIDSource base object.
     *
     * @param units The DistanceUnit that should be used.
     */
    public void setDistanceUnits(Unit units) {
        m_units = units;
    }

    /**
     * Get the current DistanceUnit that is used for the PIDSource base object.
     *
     * @return The type of DistanceUnit that is being used.
     */
    public Unit getDistanceUnits() {
        return m_units;
    }

    /**
     * Is the ultrasonic enabled
     * @return true if the ultrasonic is enabled
     */
    public boolean isEnabled() {
        return m_enabled;
    }

    /**
     * Set if the ultrasonic is enabled
     * @param enable set to true to enable the ultrasonic
     */
    public void setEnabled(boolean enable) {
        m_enabled = enable;
    }

    /*
     * Live Window code, only does anything if live window is activated.
     */
    public String getSmartDashboardType(){
        return "Ultrasonic";
    }
    private ITable m_table;
    
    /**
     * {@inheritDoc}
     */
    public void initTable(ITable subtable) {
        m_table = subtable;
        updateTable();
    }
    
    /**
     * {@inheritDoc}
     */
    public ITable getTable(){
        return m_table;
    }
    
    /**
     * {@inheritDoc}
     */
    public void updateTable() {
        if (m_table != null) {
            m_table.putNumber("Value", getRangeInches());
        }
    }
    
    /**
     * {@inheritDoc}
     */
    public void startLiveWindowMode() {}
    
    /**
     * {@inheritDoc}
     */
    public void stopLiveWindowMode() {}
}