[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/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;
+};