Encoder + Ultrasonic (Pause & Resume)

In this lesson we combine an encoder, a DC motor, and an ultrasonic sensor to make the robot move more intelligently.

The encoder measures how far the robot has travelled. The ultrasonic sensor detects whether there's an object in front of the robot. Together, the robot can move toward a chosen target distance, stop if something blocks its path, and continue once the path is clear.

Constants.h

#pragma once
namespace constant
{
  static constexpr int TITAN_ID = 42;
  static constexpr int frequency = 15600;
  // Motor channel
  static constexpr int Wheel = 0;
  // Encoder pins
  static constexpr int Wheel_VMX[2] = {0, 1};
  // Ultrasonic pins
  static constexpr int ULTRASONIC_TRIG = 12;
  static constexpr int ULTRASONIC_ECHO = 11;
}

In Robot.h we create three objects — the motor, the encoder, and the ultrasonic sensor — plus two variables: the total target distance and the ultrasonic stopping distance.

Robot.h

#pragma once
#include <frc/TimedRobot.h>
#include <frc/Encoder.h>
#include <frc/Ultrasonic.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include "studica/TitanQuad.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:
  studica::TitanQuad wheel{constant::TITAN_ID, constant::frequency, constant::Wheel};
  frc::Encoder wheelEncoder{constant::Wheel_VMX[0],constant::Wheel_VMX[1],false,frc::Encoder::k4X};
  frc::Ultrasonic ultrasonic{constant::ULTRASONIC_TRIG,constant::ULTRASONIC_ECHO};
  double targetDistance = 10000.0;
  double stopDistanceCM = 30.0;
};

In RobotInit() we set the ultrasonic sensor to automatic mode. In RobotPeriodic() we send the encoder and ultrasonic distances to SmartDashboard. In AutonomousInit() we reset the encoder so the robot always starts measuring from zero.

In AutonomousPeriodic(), the robot reads both distances. First it checks whether the target distance is reached — if so, the motor stops. If not, it checks whether an object is too close; if one is detected within the stopping distance, the robot stops and waits. Because the encoder is not reset, the robot remembers how far it has already travelled, so once the object is removed it continues the remaining distance.

This teaches an important robotics concept: the robot can pause without losing its progress. Instead of restarting the whole movement, it continues from where it stopped.

Robot.cpp

#include "Robot.h"
void Robot::RobotInit()
{
  ultrasonic.SetAutomaticMode(true);
}
void Robot::RobotPeriodic()
{
  double distanceTravelled = wheelEncoder.GetDistance();
  double ultrasonicDistanceCM = ultrasonic.GetRangeMM() / 10.0;
  frc::SmartDashboard::PutNumber("Encoder Distance", distanceTravelled);
  frc::SmartDashboard::PutNumber("Ultrasonic Distance CM", ultrasonicDistanceCM);
}
void Robot::DisabledInit() {}
void Robot::DisabledPeriodic() {}
void Robot::AutonomousInit()
{
  wheelEncoder.Reset();
}
void Robot::AutonomousPeriodic()
{
  double distanceTravelled = wheelEncoder.GetDistance();
  double ultrasonicDistanceCM = ultrasonic.GetRangeMM() / 10.0;
  bool objectDetected = ultrasonicDistanceCM > 0 && ultrasonicDistanceCM < stopDistanceCM;
  bool targetReached = distanceTravelled >= targetDistance;
  if (targetReached)
  {
    wheel.Set(0.0);
  }
  else if (objectDetected)
  {
    wheel.Set(0.0);
  }
  else
  {
    wheel.Set(1.0);
  }
}
void Robot::TeleopInit() {}
void Robot::TeleopPeriodic() {}
void Robot::TestPeriodic() {}
#ifndef RUNNING_FRC_TESTS
int main()
{
  return frc::StartRobot<Robot>();
}
#endif

Try it. Run this on a 4-wheel robot and experiment. Test different target distances and ultrasonic stopping distances to see how the robot pauses when an object is detected, then continues once the path is clear. This shows how encoder-based movement and ultrasonic obstacle detection work together in a real robot system.