From 39c8d8881a60c2af571c6d94330a725b526bdd7a Mon Sep 17 00:00:00 2001
From: aishahnazar <aishahnazar2@gmail.com>
Date: Thu, 6 Mar 2025 17:43:51 -0800
Subject: [PATCH] elevator sysid

cherry pick elevator sysid commits from original file
---
 src/main/java/frc/robot/RobotContainer.java   |  7 +++
 .../subsystems/lifter/ElevatorSubsystem.java  | 53 ++++++++++++++++++-
 2 files changed, 59 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index bce606c9..bc709f7c 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/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)));
     }
diff --git a/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java
index fd69f3b2..6861884b 100644
--- a/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java
+++ b/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);
+              }
+}
\ No newline at end of file