[examples] Add C++ RomiReference example (#2969)

Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
Co-authored-by: sciencewhiz <sciencewhiz@users.noreply.github.com>

wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/Robot.cpp

@@ -0,0 +1,71 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "Robot.h"
+
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() {
+  frc2::CommandScheduler::GetInstance().Run();
+}
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+  m_autonomousCommand = m_container.GetAutonomousCommand();
+
+  if (m_autonomousCommand != nullptr) {
+    m_autonomousCommand->Schedule();
+  }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+  // This makes sure that the autonomous stops running when
+  // teleop starts running. If you want the autonomous to
+  // continue until interrupted by another command, remove
+  // this line or comment it out.
+  if (m_autonomousCommand != nullptr) {
+    m_autonomousCommand->Cancel();
+    m_autonomousCommand = nullptr;
+  }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif

wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/RobotContainer.cpp

@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "RobotContainer.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/PrintCommand.h>
+#include <frc2/command/button/Button.h>
+
+#include "commands/TeleopArcadeDrive.h"
+
+RobotContainer::RobotContainer() {
+  // Configure the button bindings
+  ConfigureButtonBindings();
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+  // Also set default commands here
+  m_drive.SetDefaultCommand(TeleopArcadeDrive(
+      &m_drive, [this] { return m_controller.GetRawAxis(1); },
+      [this] { return -m_controller.GetRawAxis(2); }));
+
+  // Example of how to use the onboard IO
+  m_onboardButtonA.WhenPressed(frc2::PrintCommand("Button A Pressed"))
+      .WhenReleased(frc2::PrintCommand("Button A Released"));
+
+  // Setup SmartDashboard options.
+  m_chooser.SetDefaultOption("Auto Routine Distance", &m_autoDistance);
+  m_chooser.AddOption("Auto Routine Time", &m_autoTime);
+  frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+  return m_chooser.GetSelected();
+}

wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/DriveDistance.cpp

@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "commands/DriveDistance.h"
+
+#include <units/math.h>
+
+void DriveDistance::Initialize() {
+  m_drive->ArcadeDrive(0, 0);
+  m_drive->ResetEncoders();
+}
+
+void DriveDistance::Execute() {
+  m_drive->ArcadeDrive(m_speed, 0);
+}
+
+void DriveDistance::End(bool interrupted) {
+  m_drive->ArcadeDrive(0, 0);
+}
+
+bool DriveDistance::IsFinished() {
+  return units::math::abs(m_drive->GetAverageDistance()) >= m_distance;
+}

wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/DriveTime.cpp

@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "commands/DriveTime.h"
+
+void DriveTime::Initialize() {
+  m_timer.Start();
+  m_drive->ArcadeDrive(0, 0);
+}
+
+void DriveTime::Execute() {
+  m_drive->ArcadeDrive(m_speed, 0);
+}
+
+void DriveTime::End(bool interrupted) {
+  m_drive->ArcadeDrive(0, 0);
+  m_timer.Stop();
+  m_timer.Reset();
+}
+
+bool DriveTime::IsFinished() {
+  return m_timer.HasElapsed(m_duration);
+}

wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TeleopArcadeDrive.cpp

@@ -0,0 +1,20 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "commands/TeleopArcadeDrive.h"
+
+#include "subsystems/Drivetrain.h"
+
+TeleopArcadeDrive::TeleopArcadeDrive(
+    Drivetrain* subsystem, std::function<double()> xaxisSpeedSupplier,
+    std::function<double()> zaxisRotateSuppplier)
+    : m_drive{subsystem},
+      m_xaxisSpeedSupplier{xaxisSpeedSupplier},
+      m_zaxisRotateSupplier{zaxisRotateSuppplier} {
+  AddRequirements({subsystem});
+}
+
+void TeleopArcadeDrive::Execute() {
+  m_drive->ArcadeDrive(m_xaxisSpeedSupplier(), m_zaxisRotateSupplier());
+}

wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TurnDegrees.cpp

@@ -0,0 +1,39 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "commands/TurnDegrees.h"
+
+#include <units/math.h>
+#include <wpi/math>
+
+void TurnDegrees::Initialize() {
+  // Set motors to stop, read encoder values for starting point
+  m_drive->ArcadeDrive(0, 0);
+  m_drive->ResetEncoders();
+}
+
+void TurnDegrees::Execute() {
+  m_drive->ArcadeDrive(0, m_speed);
+}
+
+void TurnDegrees::End(bool interrupted) {
+  m_drive->ArcadeDrive(0, 0);
+}
+
+bool TurnDegrees::IsFinished() {
+  // Need to convert distance travelled to degrees. The Standard Romi Chassis
+  // found here https://www.pololu.com/category/203/romi-chassis-kits, has a
+  // wheel placement diameter (149 mm) - width of the wheel (8 mm) = 141 mm
+  // or 5.551 inches. We then take into consideration the width of the tires.
+  static auto inchPerDegree = (5.551_in * wpi::math::pi) / 360_deg;
+
+  // Compare distance traveled from start to distance based on degree turn.
+  return GetAverageTurningDistance() >= inchPerDegree * m_angle;
+}
+
+units::meter_t TurnDegrees::GetAverageTurningDistance() {
+  auto l = units::math::abs(m_drive->GetLeftDistance());
+  auto r = units::math::abs(m_drive->GetRightDistance());
+  return (l + r) / 2;
+}

wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TurnTime.cpp

@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "commands/TurnTime.h"
+
+void TurnTime::Initialize() {
+  m_timer.Start();
+  m_drive->ArcadeDrive(0, 0);
+}
+
+void TurnTime::Execute() {
+  m_drive->ArcadeDrive(0, m_speed);
+}
+
+void TurnTime::End(bool interrupted) {
+  m_drive->ArcadeDrive(0, 0);
+  m_timer.Stop();
+  m_timer.Reset();
+}
+
+bool TurnTime::IsFinished() {
+  return m_timer.HasElapsed(m_duration);
+}

wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/Drivetrain.cpp

@@ -0,0 +1,56 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "subsystems/Drivetrain.h"
+
+#include <wpi/math>
+
+#include "Constants.h"
+
+using namespace DriveConstants;
+
+// The Romi has the left and right motors set to
+// PWM channels 0 and 1 respectively
+// The Romi has onboard encoders that are hardcoded
+// to use DIO pins 4/5 and 6/7 for the left and right
+Drivetrain::Drivetrain() {
+  m_leftEncoder.SetDistancePerPulse(
+      wpi::math::pi * kWheelDiameter.to<double>() / kCountsPerRevolution);
+  m_rightEncoder.SetDistancePerPulse(
+      wpi::math::pi * kWheelDiameter.to<double>() / kCountsPerRevolution);
+  ResetEncoders();
+}
+
+void Drivetrain::Periodic() {
+  // This method will be called once per scheduler run.
+}
+
+void Drivetrain::ArcadeDrive(double xaxisSpeed, double zaxisRotate) {
+  m_drive.ArcadeDrive(xaxisSpeed, zaxisRotate);
+}
+
+void Drivetrain::ResetEncoders() {
+  m_leftEncoder.Reset();
+  m_rightEncoder.Reset();
+}
+
+int Drivetrain::GetLeftEncoderCount() {
+  return m_leftEncoder.Get();
+}
+
+int Drivetrain::GetRightEncoderCount() {
+  return m_rightEncoder.Get();
+}
+
+units::meter_t Drivetrain::GetLeftDistance() {
+  return units::meter_t(m_leftEncoder.GetDistance());
+}
+
+units::meter_t Drivetrain::GetRightDistance() {
+  return units::meter_t(m_rightEncoder.GetDistance());
+}
+
+units::meter_t Drivetrain::GetAverageDistance() {
+  return (GetLeftDistance() + GetRightDistance()) / 2.0;
+}

wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/OnBoardIO.cpp

@@ -0,0 +1,80 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "subsystems/OnBoardIO.h"
+
+#include <frc/DigitalInput.h>
+#include <frc/DigitalOutput.h>
+#include <frc/DriverStation.h>
+#include <frc2/Timer.h>
+
+OnBoardIO::OnBoardIO(OnBoardIO::ChannelMode dio1, OnBoardIO::ChannelMode dio2) {
+  if (dio1 == ChannelMode::INPUT) {
+    m_buttonB = std::make_unique<frc::DigitalInput>(1);
+  } else {
+    m_greenLed = std::make_unique<frc::DigitalOutput>(1);
+  }
+  if (dio2 == ChannelMode::INPUT) {
+    m_buttonC = std::make_unique<frc::DigitalInput>(2);
+    m_redLed = std::make_unique<frc::DigitalOutput>(2);
+  }
+}
+
+bool OnBoardIO::GetButtonAPressed() {
+  return m_buttonA.Get();
+}
+
+bool OnBoardIO::GetButtonBPressed() {
+  if (m_buttonB) {
+    return m_buttonB->Get();
+  }
+
+  auto currentTime = frc2::Timer::GetFPGATimestamp();
+  if (currentTime > m_nextMessageTime) {
+    frc::DriverStation::ReportError("Button B was not configured");
+    m_nextMessageTime = currentTime + kMessageInterval;
+  }
+  return false;
+}
+
+bool OnBoardIO::GetButtonCPressed() {
+  if (m_buttonC) {
+    return m_buttonC->Get();
+  }
+
+  auto currentTime = frc2::Timer::GetFPGATimestamp();
+  if (currentTime > m_nextMessageTime) {
+    frc::DriverStation::ReportError("Button C was not configured");
+    m_nextMessageTime = currentTime + kMessageInterval;
+  }
+  return false;
+}
+
+void OnBoardIO::SetGreenLed(bool value) {
+  if (m_greenLed) {
+    m_greenLed->Set(value);
+  } else {
+    auto currentTime = frc2::Timer::GetFPGATimestamp();
+    if (currentTime > m_nextMessageTime) {
+      frc::DriverStation::ReportError("Green LED was not configured");
+      m_nextMessageTime = currentTime + kMessageInterval;
+    }
+  }
+}
+
+void OnBoardIO::SetRedLed(bool value) {
+  if (m_redLed) {
+    m_redLed->Set(value);
+  } else {
+    auto currentTime = frc2::Timer::GetFPGATimestamp();
+    if (currentTime > m_nextMessageTime) {
+      frc::DriverStation::ReportError("Red LED was not configured");
+      m_nextMessageTime = currentTime + kMessageInterval;
+    }
+  }
+}
+
+void OnBoardIO::SetYellowLed(bool value) {
+  m_yellowLed.Set(value);
+}

wpilibcExamples/src/main/cpp/examples/RomiReference/include/Constants.h

@@ -0,0 +1,19 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants.  This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace DriveConstants {
+constexpr double kCountsPerRevolution = 1440.0;
+constexpr double kWheelDiameterInch = 2.75;
+}  // namespace DriveConstants

wpilibcExamples/src/main/cpp/examples/RomiReference/include/Robot.h

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
+
+#include "RobotContainer.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:
+  // Have it null by default so that if testing teleop it
+  // doesn't have undefined behavior and potentially crash.
+  frc2::Command* m_autonomousCommand = nullptr;
+  RobotContainer m_container;
+};

wpilibcExamples/src/main/cpp/examples/RomiReference/include/RobotContainer.h

@@ -0,0 +1,61 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/Joystick.h>
+#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/button/Button.h>
+
+#include "Constants.h"
+#include "commands/AutonomousDistance.h"
+#include "commands/AutonomousTime.h"
+#include "subsystems/Drivetrain.h"
+#include "subsystems/OnBoardIO.h"
+
+/**
+ * This class is where the bulk of the robot should be declared.  Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls).  Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+  // NOTE: The I/O pin functionality of the 5 exposed I/O pins depends on the
+  // hardware "overlay"
+  // that is specified when launching the wpilib-ws server on the Romi raspberry
+  // pi. By default, the following are available (listed in order from inside of
+  // the board to outside):
+  // - DIO 8 (mapped to Arduino pin 11, closest to the inside of the board)
+  // - Analog In 0 (mapped to Analog Channel 6 / Arduino Pin 4)
+  // - Analog In 1 (mapped to Analog Channel 2 / Arduino Pin 20)
+  // - PWM 2 (mapped to Arduino Pin 21)
+  // - PWM 3 (mapped to Arduino Pin 22)
+  //
+  // Your subsystem configuration should take the overlays into account
+ public:
+  RobotContainer();
+  frc2::Command* GetAutonomousCommand();
+
+ private:
+  // Assumes a gamepad plugged into channnel 0
+  frc::Joystick m_controller{0};
+  frc::SendableChooser<frc2::Command*> m_chooser;
+
+  // The robot's subsystems
+  Drivetrain m_drive;
+  OnBoardIO m_onboardIO{OnBoardIO::ChannelMode::INPUT,
+                        OnBoardIO::ChannelMode::INPUT};
+
+  // Example button
+  frc2::Button m_onboardButtonA{
+      [this] { return m_onboardIO.GetButtonAPressed(); }};
+
+  // Autonomous commands.
+  AutonomousDistance m_autoDistance{&m_drive};
+  AutonomousTime m_autoTime{&m_drive};
+
+  void ConfigureButtonBindings();
+};

wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/AutonomousDistance.h

@@ -0,0 +1,23 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/SequentialCommandGroup.h>
+
+#include "commands/DriveDistance.h"
+#include "commands/TurnDegrees.h"
+#include "subsystems/Drivetrain.h"
+
+class AutonomousDistance
+    : public frc2::CommandHelper<frc2::SequentialCommandGroup,
+                                 AutonomousDistance> {
+ public:
+  explicit AutonomousDistance(Drivetrain* drive) {
+    AddCommands(
+        DriveDistance(-0.5, 10_in, drive), TurnDegrees(-0.5, 180_deg, drive),
+        DriveDistance(-0.5, 10_in, drive), TurnDegrees(0.5, 180_deg, drive));
+  }
+};

wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/AutonomousTime.h

@@ -0,0 +1,21 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/SequentialCommandGroup.h>
+
+#include "commands/DriveTime.h"
+#include "commands/TurnTime.h"
+#include "subsystems/Drivetrain.h"
+
+class AutonomousTime
+    : public frc2::CommandHelper<frc2::SequentialCommandGroup, AutonomousTime> {
+ public:
+  explicit AutonomousTime(Drivetrain* drive) {
+    AddCommands(DriveTime(-0.6, 2_s, drive), TurnTime(-0.5, 1.3_s, drive),
+                DriveTime(-0.6, 2_s, drive), TurnTime(0.5, 1.3_s, drive));
+  }
+};

wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/DriveDistance.h

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+#include <units/length.h>
+
+#include "subsystems/Drivetrain.h"
+
+class DriveDistance
+    : public frc2::CommandHelper<frc2::CommandBase, DriveDistance> {
+ public:
+  DriveDistance(double speed, units::meter_t distance, Drivetrain* drive)
+      : m_speed(speed), m_distance(distance), m_drive(drive) {
+    AddRequirements({m_drive});
+  }
+
+  void Initialize() override;
+  void Execute() override;
+  void End(bool interrupted) override;
+  bool IsFinished() override;
+
+ private:
+  double m_speed;
+  units::meter_t m_distance;
+  Drivetrain* m_drive;
+};

wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/DriveTime.h

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc2/Timer.h>
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+#include <units/time.h>
+
+#include "subsystems/Drivetrain.h"
+
+class DriveTime : public frc2::CommandHelper<frc2::CommandBase, DriveTime> {
+ public:
+  DriveTime(double speed, units::second_t time, Drivetrain* drive)
+      : m_speed(speed), m_duration(time), m_drive(drive) {
+    AddRequirements({m_drive});
+  }
+
+  void Initialize() override;
+  void Execute() override;
+  void End(bool interrupted) override;
+  bool IsFinished() override;
+
+ private:
+  double m_speed;
+  units::second_t m_duration;
+  Drivetrain* m_drive;
+  frc2::Timer m_timer;
+};

wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/TeleopArcadeDrive.h

@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "subsystems/Drivetrain.h"
+
+class TeleopArcadeDrive
+    : public frc2::CommandHelper<frc2::CommandBase, TeleopArcadeDrive> {
+ public:
+  TeleopArcadeDrive(Drivetrain* subsystem,
+                    std::function<double()> xaxisSpeedSupplier,
+                    std::function<double()> zaxisRotateSupplier);
+  void Execute() override;
+
+ private:
+  Drivetrain* m_drive;
+  std::function<double()> m_xaxisSpeedSupplier;
+  std::function<double()> m_zaxisRotateSupplier;
+};

wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/TurnDegrees.h

@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+#include <units/angle.h>
+#include <units/length.h>
+
+#include "subsystems/Drivetrain.h"
+
+class TurnDegrees : public frc2::CommandHelper<frc2::CommandBase, TurnDegrees> {
+ public:
+  TurnDegrees(double speed, units::degree_t angle, Drivetrain* drive)
+      : m_speed(speed), m_angle(angle), m_drive(drive) {
+    AddRequirements({m_drive});
+  }
+
+  void Initialize() override;
+  void Execute() override;
+  void End(bool interrupted) override;
+  bool IsFinished() override;
+
+ private:
+  double m_speed;
+  units::degree_t m_angle;
+  Drivetrain* m_drive;
+
+  units::meter_t GetAverageTurningDistance();
+};

wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/TurnTime.h

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc2/Timer.h>
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+#include <units/time.h>
+
+#include "subsystems/Drivetrain.h"
+
+class TurnTime : public frc2::CommandHelper<frc2::CommandBase, TurnTime> {
+ public:
+  TurnTime(double speed, units::second_t time, Drivetrain* drive)
+      : m_speed(speed), m_duration(time), m_drive(drive) {
+    AddRequirements({m_drive});
+  }
+
+  void Initialize() override;
+  void Execute() override;
+  void End(bool interrupted) override;
+  bool IsFinished() override;
+
+ private:
+  double m_speed;
+  units::second_t m_duration;
+  Drivetrain* m_drive;
+  frc2::Timer m_timer;
+};

wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/Drivetrain.h

@@ -0,0 +1,81 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/Spark.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc2/command/SubsystemBase.h>
+#include <units/length.h>
+
+class Drivetrain : public frc2::SubsystemBase {
+ public:
+  static constexpr double kCountsPerRevolution = 1440.0;
+  static constexpr units::meter_t kWheelDiameter = 70_mm;
+
+  Drivetrain();
+
+  /**
+   * Will be called periodically whenever the CommandScheduler runs.
+   */
+  void Periodic() override;
+
+  /**
+   * Drives the robot using arcade controls.
+   *
+   * @param xaxisSpeed the commanded forward movement
+   * @param zaxisRotate the commanded rotation
+   */
+  void ArcadeDrive(double xaxisSpeed, double zaxisRotate);
+
+  /**
+   * Resets the drive encoders to currently read a position of 0.
+   */
+  void ResetEncoders();
+
+  /**
+   * Gets the left drive encoder count.
+   *
+   * @return the left drive encoder count
+   */
+  int GetLeftEncoderCount();
+
+  /**
+   * Gets the right drive encoder count.
+   *
+   * @return the right drive encoder count
+   */
+  int GetRightEncoderCount();
+
+  /**
+   * Gets the left distance driven.
+   *
+   * @return the left-side distance driven
+   */
+  units::meter_t GetLeftDistance();
+
+  /**
+   * Gets the right distance driven.
+   *
+   * @return the right-side distance driven
+   */
+  units::meter_t GetRightDistance();
+
+  /**
+   * Returns the average distance traveled by the left and right encoders.
+   *
+   * @return The average distance traveled by the left and right encoders.
+   */
+  units::meter_t GetAverageDistance();
+
+ private:
+  frc::Spark m_leftMotor{0};
+  frc::Spark m_rightMotor{1};
+
+  frc::Encoder m_leftEncoder{4, 5};
+  frc::Encoder m_rightEncoder{6, 7};
+
+  frc::DifferentialDrive m_drive{m_leftMotor, m_rightMotor};
+};

wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/OnBoardIO.h

@@ -0,0 +1,72 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <frc/DigitalInput.h>
+#include <frc/DigitalOutput.h>
+#include <frc2/command/SubsystemBase.h>
+
+/**
+ * This class represents the onboard IO of the Romi
+ * reference robot. This includes the pushbuttons and
+ * LEDs.
+ *
+ * <p>DIO 0 - Button A (input only)
+ * DIO 1 - Button B (input) or Green LED (output)
+ * DIO 2 - Button C (input) or Red LED (output)
+ * DIO 3 - Yellow LED (output only)
+ */
+class OnBoardIO : public frc2::SubsystemBase {
+ public:
+  enum ChannelMode { INPUT, OUTPUT };
+  OnBoardIO(OnBoardIO::ChannelMode dio1, OnBoardIO::ChannelMode dio2);
+
+  static constexpr auto kMessageInterval = 1_s;
+  units::second_t m_nextMessageTime = 0_s;
+
+  /**
+   * Gets if the A button is pressed.
+   */
+  bool GetButtonAPressed();
+
+  /**
+   * Gets if the B button is pressed.
+   */
+  bool GetButtonBPressed();
+
+  /**
+   * Gets if the C button is pressed.
+   */
+  bool GetButtonCPressed();
+
+  /**
+   * Sets the green LED.
+   */
+  void SetGreenLed(bool value);
+
+  /**
+   * Sets the red LED.
+   */
+  void SetRedLed(bool value);
+
+  /**
+   * Sets the yellow LED.
+   */
+  void SetYellowLed(bool value);
+
+ private:
+  frc::DigitalInput m_buttonA{0};
+  frc::DigitalOutput m_yellowLed{3};
+
+  // DIO 1
+  std::unique_ptr<frc::DigitalInput> m_buttonB;
+  std::unique_ptr<frc::DigitalOutput> m_greenLed;
+
+  // DIO 2
+  std::unique_ptr<frc::DigitalInput> m_buttonC;
+  std::unique_ptr<frc::DigitalOutput> m_redLed;
+};

wpilibcExamples/src/main/cpp/examples/examples.json

@@ -543,6 +543,17 @@
     "gradlebase": "cpp",
     "commandversion": 2
   },
+  {
+    "name": "RomiReference",
+    "description": "An example command-based robot program that can be used with the Romi reference robot design.",
+    "tags": [
+      "Drivetrain",
+      "Romi"
+    ],
+    "foldername": "RomiReference",
+    "gradlebase": "cppromi",
+    "commandversion": 2
+  },
   {
     "name": "StateSpaceFlywheel",
     "description": "An example state-space controller for a flywheel.",