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

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