Add angular subsystem to SysIdRoutine example

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

@@ -23,6 +23,20 @@ void SysIdRoutineBot::ConfigureBindings() {
       m_drive.SysIdDynamic(frc2::sysid::Direction::kForward));
   m_driverController.Y().WhileTrue(
       m_drive.SysIdDynamic(frc2::sysid::Direction::kReverse));
+
+  m_shooter.SetDefaultCommand(m_shooter.RunShooterCommand(
+      [this] { return constants::shooter::kShooterTargetSpeed.value(); }));
+
+  /* Uncomment to bind Shooter routine, but comment out the drive routine too
+  m_driverController.A().WhileTrue(
+      m_shooter.SysIdQuasistatic(frc2::sysid::Direction::kForward));
+  m_driverController.B().WhileTrue(
+      m_shooter.SysIdQuasistatic(frc2::sysid::Direction::kReverse));
+  m_driverController.X().WhileTrue(
+      m_shooter.SysIdDynamic(frc2::sysid::Direction::kForward));
+  m_driverController.Y().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,55 @@
+// 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_unit_t{0}};
+};

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 =
@@ -46,6 +47,16 @@ public void configureBindings() {
     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));
+
+    // Control the shooter wheel with the left trigger
+    m_shooter.setDefaultCommand(m_shooter.runShooter(m_driverController::getLeftTriggerAxis));
+
+    /* Uncomment to bind Shooter routine, but comment out the drive routine too
+    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));
+    */
   }
 
   /**

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

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

@@ -0,0 +1,128 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.sysid.subsystems;
+
+import static edu.wpi.first.units.MutableMeasure.mutable;
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.units.Angle;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.examples.sysid.Constants.ShooterConstants;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import java.util.function.DoubleSupplier;
+
+public class Shooter extends SubsystemBase {
+  // The motor on the shooter wheel .
+  private final PWMSparkMax m_shooterMotor = new PWMSparkMax(ShooterConstants.kShooterMotorPort);
+
+  // The motor on the feeder wheels.
+  private final PWMSparkMax m_feederMotor = new PWMSparkMax(ShooterConstants.kFeederMotorPort);
+
+  // The shooter wheel encoder
+  private final Encoder m_shooterEncoder =
+      new Encoder(
+          ShooterConstants.kEncoderPorts[0],
+          ShooterConstants.kEncoderPorts[1],
+          ShooterConstants.kEncoderReversed);
+
+  // Mutable holder for unit-safe voltage values, persisted to avoid reallocation.
+  private final MutableMeasure<Voltage> m_appliedVoltage = mutable(Volts.of(0));
+  // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation.
+  private final MutableMeasure<Angle> m_angle = mutable(Rotations.of(0));
+  // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation.
+  private final MutableMeasure<Velocity<Angle>> m_velocity = mutable(RotationsPerSecond.of(0));
+
+  // Create a new SysId routine for characterizing the drive.
+  private final SysIdRoutine m_sysIdRoutine =
+      new SysIdRoutine(
+          // Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage.
+          new SysIdRoutine.Config(),
+          new SysIdRoutine.Mechanism(
+              // Tell SysId how to plumb the driving voltage to the motors.
+              (Measure<Voltage> volts) -> {
+                m_shooterMotor.setVoltage(volts.in(Volts));
+              },
+              // Tell SysId how to record a frame of data for each motor on the mechanism being
+              // characterized.
+              log -> {
+                // Record a frame for the left motors.  Since these share an encoder, we consider
+                // the entire group to be one motor.
+                log.motor("shooter-wheel")
+                    .voltage(
+                        m_appliedVoltage.mut_replace(
+                            m_shooterMotor.get() * RobotController.getBatteryVoltage(), Volts))
+                    .angularPosition(m_angle.mut_replace(m_shooterEncoder.getDistance(), Rotations))
+                    .angularVelocity(
+                        m_velocity.mut_replace(m_shooterEncoder.getRate(), RotationsPerSecond));
+              },
+              // Tell SysId to make generated commands require this subsystem, suffix test state in
+              // WPILog with this subsystem's name ("drive")
+              this));
+  // PID controller to run the shooter wheel in closed-loop, set the constants equal to those
+  // calculated by SysId
+  private final PIDController m_shooterFeedback = new PIDController(ShooterConstants.kP, 0, 0);
+  // Feedforward controller to run the shooter wheel in closed-loop, set the constants equal to
+  // those calculated by SysId
+  private final SimpleMotorFeedforward m_shooterFeedforward =
+      new SimpleMotorFeedforward(
+          ShooterConstants.kSVolts, ShooterConstants.kVVoltSecondsPerRotation, 0);
+
+  /** Creates a new Drive subsystem. */
+  public Shooter() {
+    // Sets the distance per pulse for the encoders
+    m_shooterEncoder.setDistancePerPulse(ShooterConstants.kEncoderDistancePerPulse);
+  }
+
+  /**
+   * Returns a command that runs the shooter at a specifc velocity.
+   *
+   * @param shooterSpeed The commanded shooter wheel speed in rotations per second
+   */
+  public Command runShooter(DoubleSupplier shooterSpeed) {
+    // Run shooter wheel at the desired speed using a PID controller and feedforward.
+    return run(() -> {
+          m_shooterMotor.setVoltage(
+              m_shooterFeedback.calculate(m_shooterEncoder.getRate(), shooterSpeed.getAsDouble())
+                  + m_shooterFeedforward.calculate(shooterSpeed.getAsDouble()));
+          m_feederMotor.set(ShooterConstants.kFeederSpeed);
+        })
+        .finallyDo(
+            () -> {
+              m_shooterMotor.stopMotor();
+              m_feederMotor.stopMotor();
+            })
+        .withName("runShooter");
+  }
+
+  /**
+   * 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);
+  }
+}