[examples] Add angular subsystem to SysIdRoutine example (#6297)

Co-authored-by: Tim Winters <twinters@wpi.edu>

wpilibcExamples/src/main/cpp/examples/SysId/cpp/SysIdRoutineBot.cpp

@@ -15,14 +15,28 @@ void SysIdRoutineBot::ConfigureBindings() {
       [this] { return -m_driverController.GetLeftY(); },
       [this] { return -m_driverController.GetRightX(); }));
 
-  m_driverController.A().WhileTrue(
-      m_drive.SysIdQuasistatic(frc2::sysid::Direction::kForward));
-  m_driverController.B().WhileTrue(
-      m_drive.SysIdQuasistatic(frc2::sysid::Direction::kReverse));
-  m_driverController.X().WhileTrue(
-      m_drive.SysIdDynamic(frc2::sysid::Direction::kForward));
-  m_driverController.Y().WhileTrue(
-      m_drive.SysIdDynamic(frc2::sysid::Direction::kReverse));
+  // Using bumpers as a modifier and combining it with the buttons so that we
+  // can have both sets of bindings at once
+  (m_driverController.A() && m_driverController.RightBumper())
+      .WhileTrue(m_drive.SysIdQuasistatic(frc2::sysid::Direction::kForward));
+  (m_driverController.B() && m_driverController.RightBumper())
+      .WhileTrue(m_drive.SysIdQuasistatic(frc2::sysid::Direction::kReverse));
+  (m_driverController.X() && m_driverController.RightBumper())
+      .WhileTrue(m_drive.SysIdDynamic(frc2::sysid::Direction::kForward));
+  (m_driverController.Y() && m_driverController.RightBumper())
+      .WhileTrue(m_drive.SysIdDynamic(frc2::sysid::Direction::kReverse));
+
+  m_shooter.SetDefaultCommand(m_shooter.RunShooterCommand(
+      [this] { return m_driverController.GetLeftTriggerAxis(); }));
+
+  (m_driverController.A() && m_driverController.LeftBumper())
+      .WhileTrue(m_shooter.SysIdQuasistatic(frc2::sysid::Direction::kForward));
+  (m_driverController.B() && m_driverController.LeftBumper())
+      .WhileTrue(m_shooter.SysIdQuasistatic(frc2::sysid::Direction::kReverse));
+  (m_driverController.X() && m_driverController.LeftBumper())
+      .WhileTrue(m_shooter.SysIdDynamic(frc2::sysid::Direction::kForward));
+  (m_driverController.Y() && m_driverController.LeftBumper())
+      .WhileTrue(m_shooter.SysIdDynamic(frc2::sysid::Direction::kReverse));
 }
 
 frc2::CommandPtr SysIdRoutineBot::GetAutonomousCommand() {

wpilibcExamples/src/main/cpp/examples/SysId/cpp/subsystems/Shooter.cpp

@@ -0,0 +1,37 @@
+// 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/Shooter.h"
+
+#include <frc2/command/Commands.h>
+#include <units/angle.h>
+#include <units/voltage.h>
+
+Shooter::Shooter() {
+  m_shooterEncoder.SetDistancePerPulse(
+      constants::shooter::kEncoderDistancePerPulse.value());
+}
+
+frc2::CommandPtr Shooter::RunShooterCommand(
+    std::function<double()> shooterSpeed) {
+  return frc2::cmd::Run(
+             [this, shooterSpeed] {
+               m_shooterMotor.SetVoltage(
+                   units::volt_t{m_shooterFeedback.Calculate(
+                       m_shooterEncoder.GetRate(), shooterSpeed())} +
+                   m_shooterFeedforward.Calculate(
+                       units::turns_per_second_t{shooterSpeed()}));
+               m_feederMotor.Set(constants::shooter::kFeederSpeed);
+             },
+             {this})
+      .WithName("Set Shooter Speed");
+}
+
+frc2::CommandPtr Shooter::SysIdQuasistatic(frc2::sysid::Direction direction) {
+  return m_sysIdRoutine.Quasistatic(direction);
+}
+
+frc2::CommandPtr Shooter::SysIdDynamic(frc2::sysid::Direction direction) {
+  return m_sysIdRoutine.Dynamic(direction);
+}

wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h

@@ -8,6 +8,7 @@
 #include <numbers>
 
 #include <units/angle.h>
+#include <units/angular_acceleration.h>
 #include <units/angular_velocity.h>
 #include <units/length.h>
 #include <units/time.h>
@@ -38,8 +39,13 @@ using kv_unit =
                          units::inverse<units::turns>>;
 using kv_unit_t = units::unit_t<kv_unit>;
 
+using ka_unit =
+    units::compound_unit<units::volts,
+                         units::inverse<units::turns_per_second_squared>>;
+using ka_unit_t = units::unit_t<ka_unit>;
+
 inline constexpr std::array<int, 2> kEncoderPorts = {4, 5};
-inline constexpr bool kLeftEncoderReversed = false;
+inline constexpr bool kEncoderReversed = false;
 inline constexpr int kEncoderCpr = 1024;
 inline constexpr units::turn_t kEncoderDistancePerPulse =
     units::turn_t{1.0} / static_cast<double>(kEncoderCpr);
@@ -55,6 +61,7 @@ inline constexpr double kP = 1.0;
 
 inline constexpr units::volt_t kS = 0.05_V;
 inline constexpr kv_unit_t kV = (12_V) / kShooterFreeSpeed;
+inline constexpr ka_unit_t kA = 0_V * 1_s * 1_s / units::turn_t{1};
 
 inline constexpr double kFeederSpeed = 0.5;
 }  // namespace shooter

wpilibcExamples/src/main/cpp/examples/SysId/include/SysIdRoutineBot.h

@@ -9,6 +9,7 @@
 
 #include "Constants.h"
 #include "subsystems/Drive.h"
+#include "subsystems/Shooter.h"
 
 class SysIdRoutineBot {
  public:
@@ -21,4 +22,5 @@ class SysIdRoutineBot {
   frc2::CommandXboxController m_driverController{
       constants::oi::kDriverControllerPort};
   Drive m_drive;
+  Shooter m_shooter;
 };

wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Shooter.h

@@ -0,0 +1,54 @@
+// 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 <frc/Encoder.h>
+#include <frc/RobotController.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc2/command/SubsystemBase.h>
+#include <frc2/command/sysid/SysIdRoutine.h>
+
+#include "Constants.h"
+
+class Shooter : public frc2::SubsystemBase {
+ public:
+  Shooter();
+
+  frc2::CommandPtr RunShooterCommand(std::function<double()> shooterSpeed);
+  frc2::CommandPtr SysIdQuasistatic(frc2::sysid::Direction direction);
+  frc2::CommandPtr SysIdDynamic(frc2::sysid::Direction direction);
+
+ private:
+  frc::PWMSparkMax m_shooterMotor{constants::shooter::kShooterMotorPort};
+  frc::PWMSparkMax m_feederMotor{constants::shooter::kFeederMotorPort};
+
+  frc::Encoder m_shooterEncoder{constants::shooter::kEncoderPorts[0],
+                                constants::shooter::kEncoderPorts[1],
+                                constants::shooter::kEncoderReversed};
+
+  frc2::sysid::SysIdRoutine m_sysIdRoutine{
+      frc2::sysid::Config{std::nullopt, std::nullopt, std::nullopt,
+                          std::nullopt},
+      frc2::sysid::Mechanism{
+          [this](units::volt_t driveVoltage) {
+            m_shooterMotor.SetVoltage(driveVoltage);
+          },
+          [this](frc::sysid::SysIdRoutineLog* log) {
+            log->Motor("shooter-wheel")
+                .voltage(m_shooterMotor.Get() *
+                         frc::RobotController::GetBatteryVoltage())
+                .position(units::turn_t{m_shooterEncoder.GetDistance()})
+                .velocity(
+                    units::turns_per_second_t{m_shooterEncoder.GetRate()});
+          },
+          this}};
+  frc::PIDController m_shooterFeedback{constants::shooter::kP, 0, 0};
+  frc::SimpleMotorFeedforward<units::turns> m_shooterFeedforward{
+      constants::shooter::kS, constants::shooter::kV, constants::shooter::kA};
+};