- FRC Game Manual
- FRC Game Q&A

# Ultrasonics - Software

Note

This section covers ultrasonics in software. For a hardware guide to ultrasonics, see Ultrasonics - Hardware.

An ultrasonic sensor is commonly used to measure distance to an object using high-frequency sound. Generally, ultrasonics measure the distance to the closest object within their “field of view.”

There are two primary types of ultrasonics supported natively by WPILib:

- Ping-response ultrasonics

- Analog ultrasonics

## Ping-response ultrasonics

The `Ultrasonic` class (Java, C++) provides support for ping-response ultrasonics. As ping-response ultrasonics (per the name) require separate pins for both sending the ping and measuring the response, users must specify DIO pin numbers for both output and input when constructing an `Ultrasonic` instance:

Java

      // Creates a ping-response Ultrasonic object on DIO 1 and 2.
      Ultrasonic m_rangeFinder = new Ultrasonic(1, 2);

C++

      // Creates a ping-response Ultrasonic object on DIO 1 and 2.
      frc::Ultrasonic m_rangeFinder{1, 2};

The measurement can then be retrieved in either inches or millimeters in Java; in C++ the units library is used to automatically convert to any desired length unit:

Java

        // We can read the distance in millimeters
        double distanceMillimeters = m_rangeFinder.getRangeMM();
        // ... or in inches
        double distanceInches = m_rangeFinder.getRangeInches();

C++

      // We can read the distance
      units::meter_t distance = m_rangeFinder.GetRange();
      // units auto-convert
      units::millimeter_t distanceMillimeters = distance;
      units::inch_t distanceInches = distance;

## Analog ultrasonics

Some ultrasonic sensors simply return an analog voltage corresponding to the measured distance. These sensors can may simply be used with the AnalogPotentiometer class.

## Third-party ultrasonics

Other ultrasonic sensors offered by third-parties may use more complicated communications protocols (such as I2C or SPI). WPILib does not provide native support for any such ultrasonics; they will typically be controlled with vendor libraries.

## Using ultrasonics in code

Ultrasonic sensors are very useful for determining spacing during autonomous routines. For example, the following code from the UltrasonicPID example project (Java, C++) will move the robot to 1 meter away from the nearest object the sensor detects:

Java

    public class Robot extends TimedRobot {
      // distance the robot wants to stay from an object
      // (one meter)
      static final double kHoldDistanceMillimeters = 1.0e3;

      // proportional speed constant
      private static final double kP = 0.001;
      // integral speed constant
      private static final double kI = 0.0;
      // derivative speed constant
      private static final double kD = 0.0;

      static final int kLeftMotorPort = 0;
      static final int kRightMotorPort = 1;

      static final int kUltrasonicPingPort = 0;
      static final int kUltrasonicEchoPort = 1;

      // Ultrasonic sensors tend to be quite noisy and susceptible to sudden outliers,
      // so measurements are filtered with a 5-sample median filter
      private final MedianFilter m_filter = new MedianFilter(5);

      private final Ultrasonic m_ultrasonic = new Ultrasonic(kUltrasonicPingPort, kUltrasonicEchoPort);
      private final PWMSparkMax m_leftMotor = new PWMSparkMax(kLeftMotorPort);
      private final PWMSparkMax m_rightMotor = new PWMSparkMax(kRightMotorPort);
      private final DifferentialDrive m_robotDrive =
          new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
      private final PIDController m_pidController = new PIDController(kP, kI, kD);

      public Robot() {
        SendableRegistry.addChild(m_robotDrive, m_leftMotor);
        SendableRegistry.addChild(m_robotDrive, m_rightMotor);
      }

      @Override
      public void autonomousInit() {
        // Set setpoint of the pid controller
        m_pidController.setSetpoint(kHoldDistanceMillimeters);
      }

      @Override
      public void autonomousPeriodic() {
        double measurement = m_ultrasonic.getRangeMM();
        double filteredMeasurement = m_filter.calculate(measurement);
        double pidOutput = m_pidController.calculate(filteredMeasurement);

        // disable input squaring -- PID output is linear
        m_robotDrive.arcadeDrive(pidOutput, 0, false);
      }

    }

C++ (Header)

    class Robot : public frc::TimedRobot {
     public:
      Robot();
      void AutonomousInit() override;
      void AutonomousPeriodic() override;

      // distance the robot wants to stay from an object
      static constexpr units::millimeter_t kHoldDistance = 1_m;

      static constexpr int kLeftMotorPort = 0;
      static constexpr int kRightMotorPort = 1;
      static constexpr int kUltrasonicPingPort = 0;
      static constexpr int kUltrasonicEchoPort = 1;

     private:
      // proportional speed constant
      static constexpr double kP = 0.001;
      // integral speed constant
      static constexpr double kI = 0.0;
      // derivative speed constant
      static constexpr double kD = 0.0;

      // Ultrasonic sensors tend to be quite noisy and susceptible to sudden
      // outliers, so measurements are filtered with a 5-sample median filter
      frc::MedianFilter<units::millimeter_t> m_filter{5};

      frc::Ultrasonic m_ultrasonic{kUltrasonicPingPort, kUltrasonicEchoPort};
      frc::PWMSparkMax m_left{kLeftMotorPort};
      frc::PWMSparkMax m_right{kRightMotorPort};
      frc::DifferentialDrive m_robotDrive{
          [&](double output) { m_left.Set(output); },
          [&](double output) { m_right.Set(output); }};
      frc::PIDController m_pidController{kP, kI, kD};
    };

C++ (Source)

    void Robot::AutonomousInit() {
      // Set setpoint of the pid controller
      m_pidController.SetSetpoint(kHoldDistance.value());
    }

    void Robot::AutonomousPeriodic() {
      units::millimeter_t measurement = m_ultrasonic.GetRange();
      units::millimeter_t filteredMeasurement = m_filter.Calculate(measurement);
      double pidOutput = m_pidController.Calculate(filteredMeasurement.value());

      // disable input squaring -- PID output is linear
      m_robotDrive.ArcadeDrive(pidOutput, 0, false);
    }

Additionally, ping-response ultrasonics can be sent to Shuffleboard, where they will be displayed with their own widgets:

Java

        // Add the ultrasonic on the "Sensors" tab of the dashboard
        // Data will update automatically
        Shuffleboard.getTab("Sensors").add(m_rangeFinder);

C++

      // Add the ultrasonic on the "Sensors" tab of the dashboard
      // Data will update automatically
      frc::Shuffleboard::GetTab("Sensors").Add(m_rangeFinder);