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

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

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

@@ -57,6 +57,7 @@ public final class Constants {
     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 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 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 class SysIdRoutineBot {
 
     // 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 class Drive extends SubsystemBase {
         .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,129 @@
+// 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 shooter.
+  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 motor(s).
+              (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 shooter 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 ("shooter")
+              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,
+          ShooterConstants.kAVoltSecondsSquaredPerRotation);
+
+  /** Creates a new Shooter 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);
+  }
+}