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