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