[examples] Add XRP C++ Examples and Templates (wpilibsuite#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