sysid

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

@@ -7,12 +7,14 @@
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
 import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj.Notifier;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
@@ -23,12 +25,14 @@
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj.util.Color8Bit;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.lib.AdvancedSubsystem;
 import frc.lib.CTREUtil;
 import frc.lib.FaultLogger;
 import frc.robot.Constants;
 import frc.robot.Constants.IntakeConstants;
 import frc.robot.Robot;
+import frc.robot.utils.SysId;
 
 public class Intake extends AdvancedSubsystem {
   private final Mechanism2d _mech = new Mechanism2d(1.85, 1);
@@ -44,9 +48,24 @@ public class Intake extends AdvancedSubsystem {
   private final MotionMagicVoltage _actuatorPositionSetter = new MotionMagicVoltage(0);
   private final VelocityVoltage _feedVelocitySetter = new VelocityVoltage(0);
 
+  private final VoltageOut _actuatorVoltageSetter = new VoltageOut(0);
+  private final VoltageOut _feedVoltageSetter = new VoltageOut(0);
+
   private final StatusSignal<Angle> _actuatorPositionGetter = _actuatorMotor.getPosition();
   private final StatusSignal<AngularVelocity> _feedVelocityGetter = _feedMotor.getVelocity();
 
+  private final SysIdRoutine _actuatorRoutine =
+      new SysIdRoutine(
+          new SysIdRoutine.Config(),
+          new SysIdRoutine.Mechanism(
+              (Voltage volts) -> setActuatorVoltage(volts.in(Volts)), null, this));
+
+  private final SysIdRoutine _feedRoutine =
+      new SysIdRoutine(
+          new SysIdRoutine.Config(),
+          new SysIdRoutine.Mechanism(
+              (Voltage volts) -> setFeedVoltage(volts.in(Volts)), null, this));
+
   private SingleJointedArmSim _actuatorSim;
 
   private double _lastSimTime;
@@ -80,6 +99,9 @@ public Intake() {
     FaultLogger.register(_feedMotor);
     FaultLogger.register(_actuatorMotor);
 
+    SysId.displayRoutine("Actuator", _actuatorRoutine);
+    SysId.displayRoutine("Intake Feed", _feedRoutine);
+
     if (Robot.isSimulation()) {
       _actuatorSim =
           new SingleJointedArmSim(
@@ -173,6 +195,14 @@ public Command outtake() {
         .withName("Outtake");
   }
 
+  private void setActuatorVoltage(double volts) {
+    _actuatorMotor.setControl(_actuatorVoltageSetter.withOutput(volts));
+  }
+
+  private void setFeedVoltage(double volts) {
+    _feedMotor.setControl(_feedVoltageSetter.withOutput(volts));
+  }
+
   @Override
   public void periodic() {
     super.periodic();

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

@@ -16,6 +16,7 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.networktables.BooleanEntry;
 import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Notifier;
 import edu.wpi.first.wpilibj.RobotController;
@@ -26,13 +27,15 @@
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.lib.AdvancedSubsystem;
 import frc.lib.CTREUtil;
 import frc.lib.FaultLogger;
 import frc.lib.Tuning;
 import frc.robot.Constants;
 import frc.robot.Constants.ManipulatorConstants;
 import frc.robot.Robot;
+import frc.robot.utils.SysId;
 import java.util.function.Consumer;
 
 public class Manipulator extends AdvancedSubsystem {
@@ -62,6 +65,12 @@ public class Manipulator extends AdvancedSubsystem {
 
   private final StatusSignal<AngularVelocity> _feedVelocityGetter = _leftMotor.getVelocity();
 
+  private final SysIdRoutine _feedRoutine =
+      new SysIdRoutine(
+          new SysIdRoutine.Config(),
+          new SysIdRoutine.Mechanism(
+              (Voltage volts) -> setFeedVoltage(volts.in(Volts)), null, this));
+
   private DIOSim _coralBeamSim;
   private DIOSim _algaeBeamSim;
 
@@ -89,6 +98,8 @@ public Manipulator(Consumer<Piece> currentPieceSetter) {
     FaultLogger.register(_leftMotor);
     FaultLogger.register(_rightMotor);
 
+    SysId.displayRoutine("Manipulator Feed", _feedRoutine);
+
     if (Robot.isSimulation()) {
       _coralBeamSim = new DIOSim(_coralBeam);
       _algaeBeamSim = new DIOSim(_algaeBeam);
@@ -244,6 +255,10 @@ public Command pulse() {
         .until(() -> _coralEvent.rising().getAsBoolean());
   }
 
+  private void setFeedVoltage(double volts) {
+    _leftMotor.setControl(_feedVoltageSetter.withOutput(volts));
+  }
+
   @Override
   public void periodic() {
     super.periodic();

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

@@ -1,26 +1,31 @@
 package frc.robot.subsystems;
 
 import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.Volts;
 
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.networktables.BooleanEntry;
 import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.simulation.DIOSim;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.lib.AdvancedSubsystem;
 import frc.lib.CTREUtil;
 import frc.lib.FaultLogger;
 import frc.lib.Tuning;
 import frc.robot.Constants.SerializerConstants;
 import frc.robot.Robot;
 import frc.robot.subsystems.Manipulator.Piece;
+import frc.robot.utils.SysId;
 import java.util.function.Consumer;
 
 public class Serializer extends AdvancedSubsystem {
@@ -38,6 +43,14 @@ public class Serializer extends AdvancedSubsystem {
   private final VelocityVoltage _feedVelocitySetter = new VelocityVoltage(0);
   private final StatusSignal<AngularVelocity> _feedVelocityGetter = _feedMotor.getVelocity();
 
+  private final VoltageOut _feedVoltageSetter = new VoltageOut(0);
+
+  private final SysIdRoutine _feedRoutine =
+      new SysIdRoutine(
+          new SysIdRoutine.Config(),
+          new SysIdRoutine.Mechanism(
+              (Voltage volts) -> setFeedVoltage(volts.in(Volts)), null, this));
+
   private final Consumer<Piece> _currentPieceSetter;
 
   public Serializer(Consumer<Piece> currentPieceSetter) {
@@ -48,6 +61,8 @@ public Serializer(Consumer<Piece> currentPieceSetter) {
     _frontBeam = new DigitalInput(SerializerConstants.frontBeamPort);
     _backBeam = new DigitalInput(SerializerConstants.backBeamPort);
 
+    SysId.displayRoutine("Serializer Feed", _feedRoutine);
+
     if (Robot.isSimulation()) {
       _frontBeamSim = new DIOSim(_frontBeam);
       _backBeamSim = new DIOSim(_backBeam);
@@ -122,6 +137,10 @@ public Command inversePassoff() {
         .withName("Inverse Passoff");
   }
 
+  private void setFeedVoltage(double volts) {
+    _feedMotor.setControl(_feedVoltageSetter.withOutput(volts));
+  }
+
   @Override
   public void periodic() {
     super.periodic();

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

@@ -10,6 +10,7 @@
 import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage;
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.MotionMagicIsRunningValue;
 import dev.doglog.DogLog;
@@ -24,6 +25,7 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Notifier;
 import edu.wpi.first.wpilibj.RobotController;
@@ -39,6 +41,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.lib.AdvancedSubsystem;
 import frc.lib.CTREUtil;
 import frc.lib.FaultLogger;
@@ -48,6 +51,7 @@
 import frc.robot.Constants.WristevatorConstants.Preset;
 import frc.robot.Constants.WristevatorConstants.Setpoint;
 import frc.robot.Robot;
+import frc.robot.utils.SysId;
 import java.util.function.DoubleSupplier;
 
 public class Wristevator extends AdvancedSubsystem {
@@ -68,14 +72,17 @@ public class Wristevator extends AdvancedSubsystem {
   private final TalonFX _wristMotor =
       new TalonFX(WristevatorConstants.wristMotorId, Constants.canivore);
 
-  private final StatusSignal<Angle> _heightGetter = _leftMotor.getPosition();
-  private final StatusSignal<Angle> _angleGetter = _wristMotor.getPosition();
-
   private final DynamicMotionMagicVoltage _heightSetter =
       new DynamicMotionMagicVoltage(HOME.getHeight().in(Rotations), 0, 0, 0);
   private final DynamicMotionMagicVoltage _angleSetter =
       new DynamicMotionMagicVoltage(HOME.getAngle().in(Rotations), 0, 0, 0);
 
+  private final VoltageOut _elevatorVoltageSetter = new VoltageOut(0);
+  private final VoltageOut _wristVoltageSetter = new VoltageOut(0);
+
+  private final StatusSignal<Angle> _heightGetter = _leftMotor.getPosition();
+  private final StatusSignal<Angle> _angleGetter = _wristMotor.getPosition();
+
   private final StatusSignal<Double> _elevatorReference = _leftMotor.getClosedLoopReference();
   private final StatusSignal<Double> _elevatorReferenceSlope =
       _leftMotor.getClosedLoopReferenceSlope();
@@ -84,6 +91,18 @@ public class Wristevator extends AdvancedSubsystem {
   private final StatusSignal<Double> _wristReferenceSlope =
       _wristMotor.getClosedLoopReferenceSlope();
 
+  private final SysIdRoutine _elevatorRoutine =
+      new SysIdRoutine(
+          new SysIdRoutine.Config(),
+          new SysIdRoutine.Mechanism(
+              (Voltage volts) -> setElevatorVoltage(volts.in(Volts)), null, this));
+
+  private final SysIdRoutine _wristRoutine =
+      new SysIdRoutine(
+          new SysIdRoutine.Config(),
+          new SysIdRoutine.Mechanism(
+              (Voltage volts) -> setWristVoltage(volts.in(Volts)), null, this));
+
   @Logged(name = "Motion Magic Timestamp Threshold")
   private double _motionMagicTimestampThreshold = 0;
 
@@ -202,6 +221,9 @@ public Wristevator() {
     DogLog.log("Wristevator/Presets", presets);
     DogLog.log("Wristevator/Intermediates", intermediates);
 
+    SysId.displayRoutine("Elevator", _elevatorRoutine);
+    SysId.displayRoutine("Wrist", _wristRoutine);
+
     if (Robot.isSimulation()) {
       _homeSwitchSim = new DIOSim(_homeSwitch);
 
@@ -544,6 +566,14 @@ public Command setSpeeds(DoubleSupplier elevatorSpeed, DoubleSupplier wristSpeed
         .withName("Set Speeds");
   }
 
+  private void setElevatorVoltage(double volts) {
+    _leftMotor.setControl(_elevatorVoltageSetter.withOutput(volts));
+  }
+
+  private void setWristVoltage(double volts) {
+    _wristMotor.setControl(_wristVoltageSetter.withOutput(volts));
+  }
+
   @Override
   public void periodic() {
     super.periodic();