Some upgrades to add SysID characterization for the Elevator subsystem

src/main/java/frc/robot/RobotContainer.java

@@ -10,25 +10,53 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
 import frc.robot.constants.OIConstants;
 import frc.robot.constants.ElevatorConstants.ElevatorPositions;
+import frc.robot.constants.OIConstants.RobotOperatingMode;
 import frc.robot.subsystems.Elevator;
+import frc.robot.subsystems.ElevatorSysID;
 
 public class RobotContainer {
   private Elevator elevator;
 
   private CommandXboxController driver;
 
+  private RobotOperatingMode operatingMode;
+
   public RobotContainer() {
-    elevator = new Elevator();
+    operatingMode = RobotOperatingMode.kNormal;
+
+    switch(operatingMode) {
+      case kNormal:
+      case kElevatorSetpointDebug:
+      default:
+        elevator = new Elevator();
+        break;
+      case kElevatorSysIDCharacterization:
+        elevator = new ElevatorSysID();
+        break;
+    }
 
     driver = new CommandXboxController(OIConstants.kDriverUSB);
 
-    configureBindings();
+    switch(operatingMode) {
+      case kNormal:
+      default:
+        configureNormalBindings();
+        break;
+      case kElevatorSetpointDebug:
+        configureElevatorSetpointBindings();
+        break;
+      case kElevatorSysIDCharacterization:
+        configureElevatorSysIDBindings();
+        break;
+    }
+
     configureShuffleboard();
   }
 
-  private void configureBindings() {
+  private void configureNormalBindings() {
     elevator.setDefaultCommand(
       elevator.assistedManualControl(driver::getRightY)
     );
@@ -39,6 +67,30 @@ public class RobotContainer {
     driver.y().onTrue(elevator.goToPosition(ElevatorPositions.kTop, 3));
   }
 
+  private void configureElevatorSetpointBindings() {
+    elevator.setDefaultCommand(
+      elevator.debugSetpoints()
+    );
+  }
+
+  private void configureElevatorSysIDBindings() {
+    driver.a().and(driver.rightBumper()).whileTrue(
+      ((ElevatorSysID)elevator).sysIdQuasistatic(Direction.kForward)
+    );
+
+    driver.b().and(driver.rightBumper()).whileTrue(
+      ((ElevatorSysID)elevator).sysIdQuasistatic(Direction.kReverse)
+    );
+
+    driver.a().and(driver.leftBumper()).whileTrue(
+      ((ElevatorSysID)elevator).sysIdDynamic(Direction.kForward)
+    );
+
+    driver.b().and(driver.leftBumper()).whileTrue(
+      ((ElevatorSysID)elevator).sysIdDynamic(Direction.kReverse)
+    );
+  }
+
   private void configureShuffleboard() {
     ShuffleboardTab tab = Shuffleboard.getTab("MyTab");
 

src/main/java/frc/robot/constants/OIConstants.java

@@ -1,5 +1,11 @@
 package frc.robot.constants;
 
 public class OIConstants {
+    public enum RobotOperatingMode {
+        kNormal,
+        kElevatorSetpointDebug,
+        kElevatorSysIDCharacterization
+    }
+
     public static final int kDriverUSB = 0;
 }

src/main/java/frc/robot/subsystems/Elevator.java

@@ -16,10 +16,10 @@ import frc.robot.constants.ElevatorConstants;
 import frc.robot.constants.ElevatorConstants.ElevatorPositions;
 
 public class Elevator extends SubsystemBase {
-    private WPI_VictorSPX motor1;
-    private WPI_VictorSPX motor2;
+    protected WPI_VictorSPX motor1;
+    protected WPI_VictorSPX motor2;
 
-    private Encoder encoder;
+    protected Encoder encoder;
 
     private PIDController positionalController;
     private PIDController velocityController;

src/main/java/frc/robot/subsystems/ElevatorSysID.java

@@ -0,0 +1,60 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+
+public class ElevatorSysID extends Elevator {
+    private MutableMeasure<Voltage> appliedVoltage;
+    private MutableMeasure<Distance> distance;
+    private MutableMeasure<Velocity<Distance>> velocity;
+
+    private SysIdRoutine sysIDRoutine;
+
+    public ElevatorSysID() {
+        super();
+
+        appliedVoltage = MutableMeasure.mutable(Units.Volts.of(0));
+        distance = MutableMeasure.mutable(Units.Meters.of(0));
+        velocity = MutableMeasure.mutable(Units.MetersPerSecond.of(0));
+
+        sysIDRoutine = new SysIdRoutine(
+            new SysIdRoutine.Config(), 
+            new SysIdRoutine.Mechanism(
+                (Measure<Voltage> volts) -> {
+                    motor1.setVoltage(volts.in(Units.Volts));
+                }, 
+                (log) -> {
+                    log.motor("elevator")
+                        .voltage(
+                            appliedVoltage.mut_replace(
+                                motor1.get() * RobotController.getBatteryVoltage(),
+                                Units.Volts
+                            )
+                        )
+                        .linearPosition(
+                            distance.mut_replace(encoder.getDistance(), Units.Meters)
+                        )
+                        .linearVelocity(
+                            velocity.mut_replace(encoder.getRate(), Units.MetersPerSecond)
+                        );
+                }, 
+                this
+            )
+        );
+    }
+
+    public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+        return sysIDRoutine.quasistatic(direction);
+    }
+
+    public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+        return sysIDRoutine.dynamic(direction);
+    }
+}