sysid setup

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

@@ -2,9 +2,14 @@
 
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
 import frc.robot.util.LoggedTunableNumber;
+
+import static edu.wpi.first.units.Units.Volts;
+
 import org.littletonrobotics.junction.Logger;
 
 public class Elevator extends SubsystemBase {
@@ -24,6 +29,8 @@ public class Elevator extends SubsystemBase {
   private TrapezoidProfile.State extenderCurrent = new TrapezoidProfile.State();
 
   private final ElevatorFeedforward elevatorFFModel;
+
+  private final SysIdRoutine sysId;
 
   public Elevator(ElevatorIO elevator) {
     this.elevator = elevator;
@@ -47,13 +54,28 @@ public Elevator(ElevatorIO elevator) {
         break;
     }
 
+    // Configure SysId
+    sysId =
+        new SysIdRoutine(
+            new SysIdRoutine.Config(
+                null,
+                null,
+                null,
+                (state) -> Logger.recordOutput("Elevator/SysIdState", state.toString())),
+            new SysIdRoutine.Mechanism(
+                (voltage) -> {
+                  elevator.runCharacterization(voltage.in(Volts));
+                },
+                null,
+                this));
+
+
     setExtenderGoal(-3);
     extenderProfile = new TrapezoidProfile(extenderConstraints);
     extenderCurrent = extenderProfile.calculate(0, extenderCurrent, extenderGoal);
 
     extenderkP.initDefault(15);
 
-
     this.elevator.configurePID(extenderkP.get(), 0, 0);
   }
 
@@ -86,6 +108,16 @@ public double calculateAngle() {
     return angle;
   }
 
+    /** Returns a command to run a quasistatic test in the specified direction. */
+  public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+    return sysId.quasistatic(direction);
+  }
+
+  /** Returns a command to run a dynamic test in the specified direction. */
+  public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+    return sysId.dynamic(direction);
+  }
+
   @Override
   public void periodic() {
 

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

@@ -14,6 +14,8 @@ public static class ElevatorIOInputs {
 
   public default void updateInputs(ElevatorIOInputs inputs) {}
 
+  public default void runCharacterization(double volts) {}
+
   public default void setPositionSetpoint(double position, double ffVolts) {}
 
   public default void stop() {}

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

@@ -45,6 +45,11 @@ public void updateInputs(ElevatorIOInputs inputs) {
     sim.update(Constants.LOOP_PERIOD_SECS);
   }
 
+  @Override
+  public void runCharacterization(double volts) {
+    sim.setInputVoltage(volts);
+  }
+
   @Override
   public void setPositionSetpoint(double position, double ffVolts) {
     appliedVolts = ffVolts;

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

@@ -61,6 +61,11 @@ public void updateInputs(ElevatorIOInputs inputs) {
     inputs.positionSetpoint = positionSetpoint;
   }
 
+  @Override 
+  public void runCharacterization(double volts) {
+    leader.setVoltage(volts);
+  }
+
   @Override
   public void setPositionSetpoint(double position, double ffVolts) {
     this.positionSetpoint = position;

src/main/java/frc/robot/subsystems/Shooter/FeederIO.java

@@ -14,6 +14,8 @@ public static class FeederIOInputs {
 
   public default void updateInputs(FeederIOInputs inputs) {}
 
+  public default void runCharacterization(double volts) {}
+
   public default void setVelocityRPM(double velocity, double ffVolts) {}
 
   public default void stop() {}

src/main/java/frc/robot/subsystems/Shooter/FeederIOSim.java

@@ -28,6 +28,11 @@ public void updateInputs(FeederIOInputs inputs) {
     inputs.currentAmps = sim.getCurrentDrawAmps();
   }
 
+  @Override
+  public void runCharacterization(double volts) {
+    sim.setInputVoltage(volts);
+  }
+
   @Override
   public void setVelocityRPM(double velocity, double ffVolts) {
     this.ffVolts = ffVolts;

src/main/java/frc/robot/subsystems/Shooter/FeederIOSparkMax.java

@@ -31,6 +31,11 @@ public void updateInputs(FeederIOInputs inputs) {
     inputs.currentAmps = neo.getOutputCurrent();
   }
 
+  @Override
+  public void runCharacterization(double volts) {
+    neo.setVoltage(volts);
+  }
+
   @Override
   public void setVelocityRPM(double velocity, double ffVolts) {
     pid.setReference(velocity, ControlType.kVelocity, 0, ffVolts, ArbFFUnits.kVoltage);

src/main/java/frc/robot/subsystems/Shooter/FeederIOTalonFX.java

@@ -48,6 +48,11 @@ public void updateInputs(FeederIOInputs inputs) {
     inputs.currentAmps = currentAmps.getValueAsDouble();
   }
 
+  @Override
+  public void runCharacterization(double volts) {
+    falcon.setVoltage(volts);
+  }
+
   @Override
   public void setVelocityRPM(double velocity, double ffVolts) {
     this.velocitySetpoint = velocity;

src/main/java/frc/robot/subsystems/Shooter/FlywheelIOTalonFX.java

@@ -73,6 +73,12 @@ public void updateInputs(FlywheelIOInputs inputs) {
     inputs.rightVelocitySetpoint = rightSetpoint;
   }
 
+  @Override
+  public void setVoltage(double volts) {
+    left.setVoltage(volts);
+    right.setVoltage(volts);
+  }
+
   @Override
   public void setVelocityRPM(double leftVelocity, double rightVelocity, double ffVolts) {
     this.leftSetpoint = leftVelocity;

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

@@ -5,9 +5,13 @@
 package frc.robot.subsystems.shooter;
 
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
 
+import static edu.wpi.first.units.Units.Volts;
+
 import org.littletonrobotics.junction.Logger;
 
 public class Shooter extends SubsystemBase {
@@ -26,6 +30,9 @@ public class Shooter extends SubsystemBase {
   private final SimpleMotorFeedforward flywheelFFModel;
   private final SimpleMotorFeedforward feederFFModel;
 
+  private final SysIdRoutine feedSysId;
+  private final SysIdRoutine flywheelSysId;
+
   public Shooter(FlywheelIO flywheels, FeederIO feeder) {
     switch (Constants.currentMode) {
       case REAL:
@@ -46,19 +53,48 @@ public Shooter(FlywheelIO flywheels, FeederIO feeder) {
         break;
     }
     this.flywheels = flywheels;
-    // this.shooterMotor2 = shooterMotor2;
     // TODO:: Make these constants
     flywheels.configurePID(0.5, 0, 0);
-    // shooterMotor2.configurePID(0.5, 0, 0);
+
+    // Configure SysId
+    flywheelSysId =
+    new SysIdRoutine(
+        new SysIdRoutine.Config(
+            null,
+            null,
+            null,
+            (state) -> Logger.recordOutput("Flywheels/SysIdState", state.toString())),
+        new SysIdRoutine.Mechanism(
+            (voltage) -> {
+              flywheels.setVoltage(voltage.in(Volts));
+            },
+            null,
+            this));
+
 
     this.feeder = feeder;
     // TODO:: Make these constants
     feeder.configurePID(0.5, 0, 0);
+
+    // Configure SysId
+    feedSysId =
+    new SysIdRoutine(
+        new SysIdRoutine.Config(
+            null,
+            null,
+            null,
+            (state) -> Logger.recordOutput("Feeder/SysIdState", state.toString())),
+        new SysIdRoutine.Mechanism(
+            (voltage) -> {
+              feeder.runCharacterization(voltage.in(Volts));
+            },
+            null,
+            this));
+
   }
 
   public void stopShooterMotors() {
     flywheels.stop();
-    // shooterMotor2.stop();
   }
 
   public void stopFeeders() {
@@ -89,6 +125,26 @@ public boolean atFlywheelSetpoints() {
         && getFlywheelErrors()[1] <= Constants.ShooterConstants.FLYWHEEL_THRESHOLD);
   }
 
+    /** Returns a command to run a quasistatic test in the specified direction. */
+  public Command feedSysIDQuasistic(SysIdRoutine.Direction direction) {
+    return feedSysId.quasistatic(direction);
+  }
+
+  /** Returns a command to run a dynamic test in the specified direction. */
+  public Command feedSysIDDynamiCommand(SysIdRoutine.Direction direction) {
+    return feedSysId.dynamic(direction);
+  }
+
+    /** Returns a command to run a quasistatic test in the specified direction. */
+  public Command flywheelSysIDQuasistic(SysIdRoutine.Direction direction) {
+    return feedSysId.quasistatic(direction);
+  }
+
+  /** Returns a command to run a dynamic test in the specified direction. */
+  public Command flywheelSysIDDynamiCommand(SysIdRoutine.Direction direction) {
+    return feedSysId.dynamic(direction);
+  }
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run

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

@@ -67,7 +67,7 @@ public Intake(IntakeRollerIO roller) {
   }
 
   public void runRollers(double velocity) {
-    roller.setVelocity(velocity, ffModel.calculate(velocity));
+    roller.setVelocityRPM(velocity, ffModel.calculate(velocity));
   }
 
   public void stopRollers() {

src/main/java/frc/robot/subsystems/intake/IntakeRollerIO.java

@@ -10,15 +10,16 @@
 public interface IntakeRollerIO {
   @AutoLog
   public static class IntakeRollerIOInputs {
-    public double rollerVelocity = 0.0;
+    public double rollerVelocityRPM = 0.0;
     public double rollerRotations = 0.0;
     public double appliedVolts = 0.0;
     public double currentAmps = 0.0;
+    public double velocitySetpoint = 0;
   }
 
   public default void updateInputs(IntakeRollerIOInputs inputs) {}
 
-  public default void setVelocity(double velocity, double ffVolts) {}
+  public default void setVelocityRPM(double velocity, double ffVolts) {}
 
   public default void runCharacterization(double volts) {}
 

src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSim.java

@@ -14,6 +14,7 @@ public class IntakeRollerIOSim implements IntakeRollerIO {
   private boolean closedLoop = false;
   private double ffVolts = 0.0;
   private double appliedVolts = 0.0;
+  private double velocitySetpoint = 0;
 
   @Override
   public void updateInputs(IntakeRollerIOInputs inputs) {
@@ -24,14 +25,21 @@ public void updateInputs(IntakeRollerIOInputs inputs) {
     }
 
     sim.update(Constants.LOOP_PERIOD_SECS);
-    inputs.rollerVelocity = sim.getAngularVelocityRPM();
-    inputs.rollerVelocity = sim.getAngularVelocityRPM();
+    inputs.rollerVelocityRPM = sim.getAngularVelocityRPM();
+    inputs.rollerVelocityRPM = sim.getAngularVelocityRPM();
     inputs.appliedVolts = appliedVolts;
     inputs.currentAmps = sim.getCurrentDrawAmps();
+    inputs.velocitySetpoint = velocitySetpoint;
   }
 
   @Override
-  public void setVelocity(double velocity, double ffVolts) {
+  public void runCharacterization(double volts) {
+    sim.setInputVoltage(volts);
+  }
+
+  @Override
+  public void setVelocityRPM(double velocity, double ffVolts) {
+    this.velocitySetpoint = velocity;
     closedLoop = true;
     pid.setSetpoint(velocity);
     this.ffVolts = ffVolts;
@@ -40,7 +48,7 @@ public void setVelocity(double velocity, double ffVolts) {
   @Override
   public void stop() {
     closedLoop = false;
-    setVelocity(0, 0);
+    setVelocityRPM(0, 0);
   }
 
   @Override

src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSparkFlex.java

@@ -12,6 +12,8 @@ public class IntakeRollerIOSparkFlex implements IntakeRollerIO {
   private final CANSparkFlex rollers;
   private final SparkPIDController pid;
 
+  private double velocitySetpoint = 0;
+
   public IntakeRollerIOSparkFlex(int id) {
     rollers = new CANSparkFlex(id, MotorType.kBrushless);
     rollers.restoreFactoryDefaults();
@@ -27,10 +29,11 @@ public IntakeRollerIOSparkFlex(int id) {
   @Override
   public void updateInputs(IntakeRollerIOInputs inputs) {
     inputs.rollerRotations = rollers.getEncoder().getPosition();
-    inputs.rollerVelocity = rollers.getEncoder().getVelocity();
+    inputs.rollerVelocityRPM = rollers.getEncoder().getVelocity();
 
     inputs.appliedVolts = rollers.getBusVoltage();
     inputs.currentAmps = rollers.getOutputCurrent();
+    inputs.velocitySetpoint = velocitySetpoint;
   }
 
   @Override
@@ -39,7 +42,8 @@ public void runCharacterization(double volts) {
   }
 
   @Override
-  public void setVelocity(double velocity, double ffVolts) {
+  public void setVelocityRPM(double velocity, double ffVolts) {
+    this.velocitySetpoint = velocity;
     pid.setReference(velocity, ControlType.kVelocity, 0, ffVolts, ArbFFUnits.kVoltage);
   }