[examples] Add angular subsystem to SysIdRoutine example (wpilibsuite…

…#6297)

Co-authored-by: Tim Winters <[email protected]>

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

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Constants.java

@@ -57,6 +57,7 @@ public static final class ShooterConstants {
     public static final double kVVoltSecondsPerRotation =
         // Should have value 12V at free speed...
         12.0 / kShooterFreeRPS;
+    public static final double kAVoltSecondsSquaredPerRotation = 0;
 
     public static final double kFeederSpeed = 0.5;
   }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/SysIdRoutineBot.java

@@ -4,9 +4,9 @@
 
 package edu.wpi.first.wpilibj.examples.sysid;
 
-import static edu.wpi.first.wpilibj.examples.sysid.Constants.OIConstants;
-
+import edu.wpi.first.wpilibj.examples.sysid.Constants.OIConstants;
 import edu.wpi.first.wpilibj.examples.sysid.subsystems.Drive;
+import edu.wpi.first.wpilibj.examples.sysid.subsystems.Shooter;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -21,6 +21,7 @@
 public class SysIdRoutineBot {
   // The robot's subsystems
   private final Drive m_drive = new Drive();
+  private final Shooter m_shooter = new Shooter();
 
   // The driver's controller
   CommandXboxController m_driverController =
@@ -42,10 +43,44 @@ public void configureBindings() {
 
     // Bind full set of SysId routine tests to buttons; a complete routine should run each of these
     // once.
-    m_driverController.a().whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
-    m_driverController.b().whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
-    m_driverController.x().whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
-    m_driverController.y().whileTrue(m_drive.sysIdDynamic(SysIdRoutine.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()
+        .and(m_driverController.rightBumper())
+        .whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    m_driverController
+        .b()
+        .and(m_driverController.rightBumper())
+        .whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    m_driverController
+        .x()
+        .and(m_driverController.rightBumper())
+        .whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    m_driverController
+        .y()
+        .and(m_driverController.rightBumper())
+        .whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+
+    // Control the shooter wheel with the left trigger
+    m_shooter.setDefaultCommand(m_shooter.runShooter(m_driverController::getLeftTriggerAxis));
+
+    m_driverController
+        .a()
+        .and(m_driverController.leftBumper())
+        .whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    m_driverController
+        .b()
+        .and(m_driverController.leftBumper())
+        .whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    m_driverController
+        .x()
+        .and(m_driverController.leftBumper())
+        .whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    m_driverController
+        .y()
+        .and(m_driverController.leftBumper())
+        .whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
   }
 
   /**

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Drive.java

@@ -122,10 +122,20 @@ public Command arcadeDriveCommand(DoubleSupplier fwd, DoubleSupplier rot) {
         .withName("arcadeDrive");
   }
 
+  /**
+   * Returns a command that will execute a quasistatic test in the given direction.
+   *
+   * @param direction The direction (forward or reverse) to run the test in
+   */
   public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
     return m_sysIdRoutine.quasistatic(direction);
   }
 
+  /**
+   * Returns a command that will execute a dynamic test in the given direction.
+   *
+   * @param direction The direction (forward or reverse) to run the test in
+   */
   public Command sysIdDynamic(SysIdRoutine.Direction direction) {
     return m_sysIdRoutine.dynamic(direction);
   }