sysid testing

Author: woopers6

src/main/java/frc/robot/Robot.java

@@ -25,6 +25,7 @@ public Robot() {
       BuildConstants.MAVEN_NAME,
       BuildConstants.GIT_SHA,
       BuildConstants.BUILD_DATE,
+      true,
       true
     );
     m_robotContainer = new RobotContainer();

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

@@ -9,21 +9,28 @@
 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;
 import frc.robot.subsystems.drivetrain.DriveSubsystem;
+import frc.robot.subsystems.lift.LiftSubsystem;
 
 public class RobotContainer {
   private final CommandXboxController joystick = new CommandXboxController(0);
 
   private final Telemetry logger = new Telemetry(Constants.Drive.MAX_SPEED.in(MetersPerSecond));
 
   private final DriveSubsystem DRIVE_SUBSYSTEM = new DriveSubsystem(DriveSubsystem.initializeHardware(), logger);
+  private final LiftSubsystem LIFT_SUBSYSTEM = LiftSubsystem.getInstance(LiftSubsystem.initializeHardware());
 
   public RobotContainer() {
     configureBindings();
   }
 
   private void configureBindings() {
     DRIVE_SUBSYSTEM.bindControls(() -> -joystick.getLeftY(), () -> -joystick.getLeftX(), () -> -joystick.getRightX());
+    joystick.a().whileTrue(LIFT_SUBSYSTEM.getElevatorSysIDRoutine().dynamic(SysIdRoutine.Direction.kForward));
+    joystick.b().whileTrue(LIFT_SUBSYSTEM.getElevatorSysIDRoutine().dynamic(SysIdRoutine.Direction.kReverse));
+    joystick.x().whileTrue(LIFT_SUBSYSTEM.getElevatorSysIDRoutine().quasistatic(SysIdRoutine.Direction.kForward));
+    joystick.y().whileTrue(LIFT_SUBSYSTEM.getElevatorSysIDRoutine().quasistatic(SysIdRoutine.Direction.kReverse));
   }
 
   public Command getAutonomousCommand() {

src/main/java/frc/robot/subsystems/lift/LiftSubsystem.java

@@ -4,26 +4,34 @@
 import static edu.wpi.first.units.Units.Degrees;
 import static edu.wpi.first.units.Units.Inches;
 import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.Millimeters;
 import static edu.wpi.first.units.Units.Radians;
 import static edu.wpi.first.units.Units.Rotations;
-import static edu.wpi.first.units.Units.Millimeters;
+import static edu.wpi.first.units.Units.Volts;
+
+import java.util.function.Consumer;
 
 import org.lasarobotics.fsm.StateMachine;
 import org.lasarobotics.fsm.SystemState;
 import org.lasarobotics.hardware.ctre.TalonFX;
 import org.lasarobotics.hardware.generic.LimitSwitch;
 import org.lasarobotics.hardware.generic.LimitSwitch.SwitchPolarity;
 
+import com.ctre.phoenix6.SignalLogger;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.DutyCycleOut;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
 import com.ctre.phoenix6.signals.GravityTypeValue;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
+import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog.State;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
 
 public class LiftSubsystem extends StateMachine implements AutoCloseable {
@@ -734,6 +742,11 @@ public SystemState nextState() {
   private final MotionMagicVoltage m_pivotPositionSetter;
   private final MotionMagicVoltage m_elevatorPositionSetter;
   private final LimitSwitch m_elevatorHomingBeamBreak;
+  private final Consumer<SysIdRoutineLog.State> m_elevatorSysIDLogConsumer;
+  private final Consumer<SysIdRoutineLog.State> m_pivotSysIDLogConsumer;
+  private static final String ELEVATOR_MOTOR_SYSID_STATE_LOG_ENTRY = "/ElevatorMotorSysIDTestState";
+  private static final String PIVOT_MOTOR_SYSID_STATE_LOG_ENTRY = "/PivotMotorSysIDTestState";
+
 
   /** Creates a new LiftSubsystem */
   private LiftSubsystem(Hardware liftHardware) {
@@ -809,6 +822,9 @@ private LiftSubsystem(Hardware liftHardware) {
     pivotConfig.Slot0.kS = 0;
     pivotConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
 
+    m_elevatorSysIDLogConsumer = state -> SignalLogger.writeString(getName() + ELEVATOR_MOTOR_SYSID_STATE_LOG_ENTRY, state.toString());
+    m_pivotSysIDLogConsumer = state -> SignalLogger.writeString(getName() + PIVOT_MOTOR_SYSID_STATE_LOG_ENTRY, state.toString());
+
     // Apply configs for TalonFX motors
     m_elevatorMotor.applyConfigs(elevatorConfig);
     m_pivotMotor.applyConfigs(pivotConfig);
@@ -919,6 +935,43 @@ public boolean armAt(Angle targetAngle) {
     return (currentAngle.isNear(targetAngle, ARM_TOLERANCE));
   }
 
+  /**
+   * Gets the SysID routine for the elevator
+   */
+  public SysIdRoutine getElevatorSysIDRoutine() {
+    return new SysIdRoutine(
+      new SysIdRoutine.Config(
+        null,
+        Volts.of(4),
+        null,
+        m_elevatorSysIDLogConsumer
+      ),
+      new SysIdRoutine.Mechanism(
+      voltage -> m_elevatorMotor.setControl(new VoltageOut(voltage)),
+      null, s_liftinstance)
+    );
+  }
+
+
+  /**
+   * 
+   * @return
+   */
+  public SysIdRoutine getPivotSysIDRoutine() {
+    return new SysIdRoutine(
+      new SysIdRoutine.Config(
+        null,
+        Volts.of(4),
+        null,
+        m_pivotSysIDLogConsumer
+      ),
+      new SysIdRoutine.Mechanism(
+      voltage -> m_pivotMotor.setControl(new VoltageOut(voltage)),
+      null, s_liftinstance)
+    );
+  }
+
+
   /**
    * Stop the elevator motor
    */