In this lesson we use an ultrasonic sensor to measure distance. An ultrasonic sensor sends out a sound wave and measures how long it takes to bounce back after hitting an object; from that time, the robot calculates the distance between the sensor and the object.

This is useful when we want the robot to detect how close it is to a wall, a game piece, or another object. It's commonly used for simple autonomous stopping, alignment, and distance checking.

We begin by setting up the sensor ports in Constants.h — the ping and echo ports. Keeping these in one place makes the code easier to read and update.

Constants.h

// Constants.h
#pragma once
namespace constant
{
  static constexpr int TITAN_ID = 42;
  static constexpr int frequency = 15600;
  // Ultrasonic sensor ports
  static constexpr int ULTRASONIC_TRIG = 12;
  static constexpr int ULTRASONIC_ECHO = 11;
}

In Robot.h we create the ultrasonic sensor object, telling the robot which digital ports the sensor is connected to.

Robot.h

// Robot.h
#pragma once
#include <frc/TimedRobot.h>
#include <frc/Ultrasonic.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include "Constants.h"
class Robot : public frc::TimedRobot {
public:
  void RobotInit() override;
  void RobotPeriodic() override;
  void DisabledInit() override;
  void DisabledPeriodic() override;
  void AutonomousInit() override;
  void AutonomousPeriodic() override;
  void TeleopInit() override;
  void TeleopPeriodic() override;
  void TestPeriodic() override;
private:
  frc::Ultrasonic ultrasonic{constant::ULTRASONIC_TRIG, constant::ULTRASONIC_ECHO};
};

In Robot.cpp, inside RobotInit() we enable automatic mode so the sensor takes readings continuously without manual triggering. In RobotPeriodic() we send the distance to SmartDashboard in both millimetres and inches, so you can see the readings live while the robot runs.

Robot.cpp

// Robot.cpp
#include "Robot.h"
void Robot::RobotInit()
{
  ultrasonic.SetAutomaticMode(true);
}
void Robot::RobotPeriodic()
{
  frc::SmartDashboard::PutNumber("Ultrasonic Distance mm", ultrasonic.GetRangeMM());
  frc::SmartDashboard::PutNumber("Ultrasonic Distance in", ultrasonic.GetRangeInches());
}
void Robot::DisabledInit() {}
void Robot::DisabledPeriodic() {}
void Robot::AutonomousInit() {}
void Robot::AutonomousPeriodic() {}
void Robot::TeleopInit() {}
void Robot::TeleopPeriodic() {}
void Robot::TestPeriodic() {}
#ifndef RUNNING_FRC_TESTS
int main()
{
  return frc::StartRobot<Robot>();
}
#endif