[examples] Add XRP C++ Examples and Templates (#5743)

wpilibcExamples/src/main/cpp/examples/XRPReference/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 20 ms, 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/XRPReference/cpp/RobotContainer.cpp

@@ -0,0 +1,44 @@
+// 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/Commands.h>
+#include <frc2/command/button/JoystickButton.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_userButton.OnTrue(frc2::cmd::Print("USER Button Pressed"))
+      .OnFalse(frc2::cmd::Print("USER Button Released"));
+
+  frc2::JoystickButton(&m_controller, 1)
+      .OnTrue(frc2::cmd::RunOnce([this] { m_arm.SetAngle(45.0); }, {}))
+      .OnFalse(frc2::cmd::RunOnce([this] { m_arm.SetAngle(0.0); }, {}));
+
+  frc2::JoystickButton(&m_controller, 2)
+      .OnTrue(frc2::cmd::RunOnce([this] { m_arm.SetAngle(90.0); }, {}))
+      .OnFalse(frc2::cmd::RunOnce([this] { m_arm.SetAngle(0.0); }, {}));
+
+  // 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/XRPReference/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/XRPReference/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/XRPReference/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/XRPReference/cpp/commands/TurnDegrees.cpp

@@ -0,0 +1,40 @@
+// 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 <numbers>
+
+#include <units/math.h>
+
+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 XRP Chassis
+  // found here https://www.sparkfun.com/products/22230, has a
+  // wheel placement diameter (163 mm) - width of the wheel (8 mm) = 155 mm
+  // or 6.102 inches. We then take into consideration the width of the tires.
+  static auto inchPerDegree = (6.102_in * std::numbers::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/XRPReference/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/XRPReference/cpp/subsystems/Arm.cpp

@@ -0,0 +1,13 @@
+// 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/Arm.h"
+
+void Arm::Periodic() {
+  // This method will be called once per scheduler run.
+}
+
+void Arm::SetAngle(double angleDeg) {
+  m_armServo.SetAngle(angleDeg);
+}

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

@@ -0,0 +1,89 @@
+// 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 <numbers>
+
+#include "Constants.h"
+
+using namespace DriveConstants;
+
+// The XRP has the left and right motors set to
+// PWM channels 0 and 1 respectively
+// The XRP has onboard encoders that are hardcoded
+// to use DIO pins 4/5 and 6/7 for the left and right
+Drivetrain::Drivetrain() {
+  // We need to invert one side of the drivetrain so that positive voltages
+  // result in both sides moving forward. Depending on how your robot's
+  // gearbox is constructed, you might have to invert the left side instead.
+  m_rightMotor.SetInverted(true);
+
+  m_leftEncoder.SetDistancePerPulse(std::numbers::pi * kWheelDiameter.value() /
+                                    kCountsPerRevolution);
+  m_rightEncoder.SetDistancePerPulse(std::numbers::pi * kWheelDiameter.value() /
+                                     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;
+}
+
+double Drivetrain::GetAccelX() {
+  return m_accelerometer.GetX();
+}
+
+double Drivetrain::GetAccelY() {
+  return m_accelerometer.GetY();
+}
+
+double Drivetrain::GetAccelZ() {
+  return m_accelerometer.GetZ();
+}
+
+double Drivetrain::GetGyroAngleX() {
+  return m_gyro.GetAngleX();
+}
+
+double Drivetrain::GetGyroAngleY() {
+  return m_gyro.GetAngleY();
+}
+
+double Drivetrain::GetGyroAngleZ() {
+  return m_gyro.GetAngleZ();
+}
+
+void Drivetrain::ResetGyro() {
+  m_gyro.Reset();
+}

wpilibcExamples/src/main/cpp/examples/XRPReference/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/XRPReference/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/XRPReference/include/RobotContainer.h

@@ -0,0 +1,63 @@
+// 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 <frc/xrp/XRPOnBoardIO.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/CommandPtr.h>
+#include <frc2/command/button/Trigger.h>
+
+#include "Constants.h"
+#include "commands/AutonomousDistance.h"
+#include "commands/AutonomousTime.h"
+#include "subsystems/Arm.h"
+#include "subsystems/Drivetrain.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 channel 0
+  frc::Joystick m_controller{0};
+  frc::SendableChooser<frc2::Command*> m_chooser;
+
+  // The robot's subsystems
+  Drivetrain m_drive;
+  Arm m_arm;
+  frc::XRPOnBoardIO m_onboardIO;
+
+  // Example button
+  frc2::Trigger m_userButton{
+      [this] { return m_onboardIO.GetUserButtonPressed(); }};
+
+  // Autonomous commands.
+  AutonomousDistance m_autoDistance{&m_drive};
+  AutonomousTime m_autoTime{&m_drive};
+
+  void ConfigureButtonBindings();
+};

wpilibcExamples/src/main/cpp/examples/XRPReference/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/XRPReference/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/XRPReference/include/commands/DriveDistance.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 <frc2/command/Command.h>
+#include <frc2/command/CommandHelper.h>
+#include <units/length.h>
+
+#include "subsystems/Drivetrain.h"
+
+class DriveDistance : public frc2::CommandHelper<frc2::Command, 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/XRPReference/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 <frc/Timer.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/CommandHelper.h>
+#include <units/time.h>
+
+#include "subsystems/Drivetrain.h"
+
+class DriveTime : public frc2::CommandHelper<frc2::Command, 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;
+  frc::Timer m_timer;
+};

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

@@ -0,0 +1,26 @@
+// 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 <functional>
+
+#include <frc2/command/Command.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "subsystems/Drivetrain.h"
+
+class TeleopArcadeDrive
+    : public frc2::CommandHelper<frc2::Command, 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/XRPReference/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/Command.h>
+#include <frc2/command/CommandHelper.h>
+#include <units/angle.h>
+#include <units/length.h>
+
+#include "subsystems/Drivetrain.h"
+
+class TurnDegrees : public frc2::CommandHelper<frc2::Command, 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/XRPReference/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 <frc/Timer.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/CommandHelper.h>
+#include <units/time.h>
+
+#include "subsystems/Drivetrain.h"
+
+class TurnTime : public frc2::CommandHelper<frc2::Command, 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;
+  frc::Timer m_timer;
+};

wpilibcExamples/src/main/cpp/examples/XRPReference/include/subsystems/Arm.h

@@ -0,0 +1,26 @@
+// 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/xrp/XRPServo.h>
+#include <frc2/command/SubsystemBase.h>
+
+class Arm : public frc2::SubsystemBase {
+ public:
+  /**
+   * Will be called periodically whenever the CommandScheduler runs.
+   */
+  void Periodic() override;
+
+  /**
+   * Set the current angle of the arm (0 - 180 degrees).
+   *
+   * @param angleDeg the commanded angle
+   */
+  void SetAngle(double angleDeg);
+
+ private:
+  frc::XRPServo m_armServo{4};
+};

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

@@ -0,0 +1,125 @@
+// 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/BuiltInAccelerometer.h>
+#include <frc/Encoder.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/xrp/XRPGyro.h>
+#include <frc/xrp/XRPMotor.h>
+#include <frc2/command/SubsystemBase.h>
+#include <units/length.h>
+
+class Drivetrain : public frc2::SubsystemBase {
+ public:
+  static constexpr double kGearRatio =
+      (30.0 / 14.0) * (28.0 / 16.0) * (36.0 / 9.0) * (26.0 / 8.0);  // 48.75:1
+  static constexpr double kCountsPerMotorShaftRev = 12.0;
+  static constexpr double kCountsPerRevolution =
+      kCountsPerMotorShaftRev * kGearRatio;  // 585.0
+  static constexpr units::meter_t kWheelDiameter = 60_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();
+
+  /**
+   * Returns the acceleration along the X-axis, in Gs.
+   */
+  double GetAccelX();
+
+  /**
+   * Returns the acceleration along the Y-axis, in Gs.
+   */
+  double GetAccelY();
+
+  /**
+   * Returns the acceleration along the Z-axis, in Gs.
+   */
+  double GetAccelZ();
+
+  /**
+   * Returns the current angle of the Romi around the X-axis, in degrees.
+   */
+  double GetGyroAngleX();
+
+  /**
+   * Returns the current angle of the Romi around the Y-axis, in degrees.
+   */
+  double GetGyroAngleY();
+
+  /**
+   * Returns the current angle of the Romi around the Z-axis, in degrees.
+   */
+  double GetGyroAngleZ();
+
+  /**
+   * Reset the gyro.
+   */
+  void ResetGyro();
+
+ private:
+  frc::XRPMotor m_leftMotor{0};
+  frc::XRPMotor m_rightMotor{1};
+
+  frc::Encoder m_leftEncoder{4, 5};
+  frc::Encoder m_rightEncoder{6, 7};
+
+  frc::DifferentialDrive m_drive{m_leftMotor, m_rightMotor};
+
+  frc::XRPGyro m_gyro;
+  frc::BuiltInAccelerometer m_accelerometer;
+};

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

@@ -691,6 +691,23 @@
       "romi"
     ]
   },
+  {
+    "name": "XRP Reference",
+    "description": "An example command-based robot program that can be used with the XRP reference robot design.",
+    "tags": [
+      "XRP",
+      "Command-based",
+      "Differential Drive",
+      "Digital Input",
+      "Joystick"
+    ],
+    "foldername": "XRPReference",
+    "gradlebase": "cppxrp",
+    "commandversion": 2,
+    "extravendordeps": [
+      "xrp"
+    ]
+  },
   {
     "name": "StateSpaceFlywheel",
     "description": "Control a flywheel using a state-space model (based on values from CAD), with a Kalman Filter and LQR.",

wpilibcExamples/src/main/cpp/templates/templates.json

@@ -100,5 +100,33 @@
     "extravendordeps": [
       "romi"
     ]
+  },
+  {
+    "name": "XRP - Command Robot",
+    "description": "XRP - Command style",
+    "tags": [
+      "Command",
+      "XRP"
+    ],
+    "foldername": "commandbased",
+    "gradlebase": "cppxrp",
+    "commandversion": 2,
+    "extravendordeps": [
+      "xrp"
+    ]
+  },
+  {
+    "name": "XRP - Timed Robot",
+    "description": "XRP - Timed style",
+    "tags": [
+      "Timed",
+      "XRP"
+    ],
+    "foldername": "timed",
+    "gradlebase": "cppxrp",
+    "commandversion": 2,
+    "extravendordeps": [
+      "xrp"
+    ]
   }
 ]