In this lesson we control a servo motor using a basic autonomous program. A servo doesn't spin continuously like a normal DC motor — it moves to a specific angle. That makes it useful for accurate movement in mechanisms like robotic arms, selectors, flaps, or steering systems.

Version 1 — Angles Stored as Constants

Version 1

We begin by setting up our constants in Constants.h. This is where we store values such as the servo port and the angles we want the servo to move to. Storing them as constants makes the code easier to understand and update later.

Constants.h

#pragma once
// Defines a namespace to hold constant values
namespace constant
{
  // Titan controller CAN ID
  static constexpr int TITAN_ID = 42;
  // Communication frequency for the Titan controller
  static constexpr int frequency = 15600;
  // PWM port used by the servo motor
  static constexpr int DIF_SERVO = 0;
  // Maximum servo angle in degrees
  static constexpr double SERVO_MAX_ANGLE = 300.0;
  // Servo positions for this example
  static constexpr double SERVO_START_ANGLE = 0.0;
  static constexpr double SERVO_TARGET_ANGLE = 300.0;
}

In Robot.h we create the servo object (telling the robot which PWM port the servo is on) and add a timer, because we want the servo to move based on time during autonomous.

Robot.h

#pragma once
#include <frc/TimedRobot.h>
#include <frc2/command/Command.h>
#include "RobotContainer.h"
#include <frc/Servo.h>
#include <frc/Timer.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:
  // Servo object connected to PWM port 0
  frc::Servo diffServo{constant::DIF_SERVO};
  // Timer used to control movement timing in autonomous
  frc::Timer m_timer;
};

In Robot.cpp, we set the servo to its starting angle in RobotInit() so it begins in a known position. In AutonomousInit() we reset and start the timer, and set the servo to the starting angle again. In AutonomousPeriodic() we check the timer: for the first 2 seconds we move to the target angle, then return to 0 — a simple back-and-forth movement.

Robot.cpp

#include "Robot.h"
void Robot::RobotInit()
{
  // Set the servo to its starting position when the robot first turns on
  diffServo.SetAngle(constant::SERVO_START_ANGLE);
}
void Robot::RobotPeriodic() {}
void Robot::DisabledInit() {}
void Robot::DisabledPeriodic() {}
void Robot::AutonomousInit()
{
  // Reset and start the timer at the beginning of autonomous
  m_timer.Reset();
  m_timer.Start();
  // Move the servo to the starting angle
  diffServo.SetAngle(constant::SERVO_START_ANGLE);
}
void Robot::AutonomousPeriodic()
{
  // For the first 2 seconds, move the servo to the target angle
  if (m_timer.Get() < 2.0)
  {
    diffServo.SetAngle(constant::SERVO_TARGET_ANGLE);
  }
  else
  {
    // After 2 seconds, return the servo to 0 degrees
    diffServo.SetAngle(constant::SERVO_START_ANGLE);
  }
}
void Robot::TeleopInit() {}
void Robot::TeleopPeriodic() {}
void Robot::TestPeriodic() {}
#ifndef RUNNING_FRC_TESTS
int main()
{
  return frc::StartRobot<Robot>();
}
#endif

Version 2 — Angle Set Directly in the Code

Version 2

In this version we don't rely on multiple constants or external inputs. Instead we define the angle and timing directly inside the autonomous code, making it very clear where changes should be made.

Constants.h (slightly changed)

// Constants.h
#pragma once
namespace constant
{
  static constexpr int TITAN_ID = 42;
  static constexpr int frequency = 15600;
  // PWM port for the servo
  static constexpr int DIF_SERVO = 0;
  static constexpr double SERVO_MAX_ANGLE = 300.0;
}

Robot.h

// Robot.h
#pragma once
#include <frc/TimedRobot.h>
#include <frc/Servo.h>
#include <frc/Timer.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::Servo diffServo{constant::DIF_SERVO};
  frc::Timer m_timer;
};

Here RobotInit() is left empty because we handle setup inside autonomous. In AutonomousInit() we reset and start the timer and set the servo to 0 degrees. In AutonomousPeriodic() we define two values you can change: the target angle and how long to wait there.

Robot.cpp

#include "Robot.h"
void Robot::RobotInit()
{}
void Robot::RobotPeriodic()
{
}
void Robot::DisabledInit() {}
void Robot::DisabledPeriodic() {}
void Robot::AutonomousInit()
{
  m_timer.Reset();
  m_timer.Start();
  // Start at 0 degrees
  diffServo.SetAngle(0);
}
void Robot::AutonomousPeriodic()
{
  // ===== CHANGE YOUR ANGLE HERE =====
  double angle = 300.0;
  // ===== CHANGE YOUR WAIT TIME HERE =====
  double waitTime = 3.0;
  if (m_timer.Get() < waitTime)
  {
    diffServo.SetAngle(angle);
  }
  else
  {
    diffServo.SetAngle(0);
  }
}
void Robot::TeleopInit() {}
void Robot::TeleopPeriodic() {}
void Robot::TestPeriodic() {}
#ifndef RUNNING_FRC_TESTS
int main()
{
  return frc::StartRobot<Robot>();
}
#endif