elevator sysid

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

@@ -280,6 +280,13 @@ private void configureBindings() {
     testJoystick.leftBumper().and(testJoystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse));
     testJoystick.rightBumper().and(testJoystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward));
     testJoystick.rightBumper().and(testJoystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse));
+
+    //run sysID routines for elevator
+    testJoystick.a().whileTrue(m_elevator.sysIdDynamic(Direction.kForward));
+    testJoystick.b().whileTrue(m_elevator.sysIdDynamic(Direction.kReverse));
+    testJoystick.x().whileTrue(m_elevator.sysIdQuasistatic(Direction.kForward));
+    testJoystick.y().whileTrue(m_elevator.sysIdQuasistatic(Direction.kReverse));
+
     if (Utils.isSimulation()) {
       drivetrain.resetPose(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90)));
     }

src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java

@@ -14,6 +14,23 @@
 import java.util.function.Supplier;
 import frc.robot.Constants.ElevatorConstants;
 import frc.robot.Constants.RobotStateConstants;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Seconds;
+import static edu.wpi.first.units.Units.Volts;
+
+import com.ctre.phoenix6.SignalLogger;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+import edu.wpi.first.units.measure.MutAngle;
+import edu.wpi.first.units.measure.MutAngularVelocity;
+import edu.wpi.first.units.measure.MutDistance;
+import edu.wpi.first.units.measure.MutLinearVelocity;
+import edu.wpi.first.units.measure.MutVelocity;
+import edu.wpi.first.units.measure.MutVoltage;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 
 public class ElevatorSubsystem extends SubsystemBase{
     private final ElevatorIOHardware m_IO = new ElevatorIOHardware();
@@ -187,4 +204,38 @@ public Trigger isTallTrigger() {
     public boolean isTall() {
         return m_IO.getPosition() > ElevatorConstants.MIN_HEIGHT_TO_BE_TALL;
     }
-}
+    //ELEVATOR SYSID
+    // Mutable holder for unit-safe voltage values, persisted to avoid reallocation.
+    private final MutVoltage m_appliedVoltage = Volts.mutable(0);
+    // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation.
+    private final MutDistance m_distance = Meters.mutable(0);
+    // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation.
+    private final MutLinearVelocity m_velocity = MetersPerSecond.mutable(0);
+
+
+    private final SysIdRoutine m_sysIDRoutineElevatorRoutine = new SysIdRoutine(
+        new SysIdRoutine.Config(Volts.per(Seconds).of(0.5), Volts.of(3), Seconds.of(10)),
+        new SysIdRoutine.Mechanism(
+            output -> m_IO.setVoltage(output.in(Volts)), 
+            log -> {
+                // Record a frame for the shooter motor.
+                log.motor("elevatorsysID")
+                    .voltage(
+                        m_appliedVoltage.mut_replace(
+                            m_IO.getVoltage(), Volts))
+                            .linearPosition(m_distance.mut_replace(m_IO.getPosition(), Meters))
+                            .linearVelocity(
+                                m_velocity.mut_replace(m_IO.getVelocity(), MetersPerSecond));
+                      },
+                    this
+                )
+            );
+
+            public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+                return m_sysIDRoutineElevatorRoutine.quasistatic(direction);
+              }
+
+              public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+                return m_sysIDRoutineElevatorRoutine.dynamic(direction);
+              }
+}