sysid routines

TitaniumTitans · Jun 14, 2024 · bc1be2a · bc1be2a
1 parent 742b2c1
commit bc1be2a
Show file tree

Hide file tree

Showing 3 changed files with 38 additions and 12 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -230,14 +230,6 @@ private void configureButtonBindings() {
     // 96.240234375
     // 60.029296875
     // 2250
-//    m_shooter.setDefaultCommand(
-//        Commands.run(() -> {
-//          if (!m_climber.getClimberLock() && m_shooter.hasPiece()) {
-//            m_shooter.runShooterVelocity(false, () -> 1000, () -> 1000).execute();
-//          } else {
-//            m_shooter.setShooterPowerFactory(0.0, 0.0, 0.0);
-//          }
-//        }, m_shooter));
     m_driveSubsystem.setDefaultCommand(
         DriveCommands.joystickDrive(
                 m_driveSubsystem,
@@ -271,8 +263,6 @@ private void configureButtonBindings() {
 
     m_operatorController.pov(0).whileTrue(m_climber.setClimberPowerFactory(0.5));
     m_operatorController.pov(180).whileTrue(m_climber.setClimberPowerFactory(-0.5));
-//    m_operatorController.pov(90).whileTrue(m_climber.setRightClimberPowerFactory(0.5));
-//    m_operatorController.pov(270).whileTrue(m_climber.setRightClimberPowerFactory(-0.5));
 
     m_operatorController.a()
         .onTrue(Commands.runOnce(() -> m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.PREPARE_TRAP),

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java
@@ -14,6 +14,7 @@
 import lib.properties.phoenix6.Phoenix6PidPropertyBuilder;
 import lib.properties.phoenix6.PidPropertyPublic;
 import frc.robot.Constants.ShooterConstants;
+import org.littletonrobotics.junction.Logger;
 
 import java.util.Arrays;
 
@@ -171,6 +172,9 @@ public void updateInputs(ShooterIOInputsAutoLogged inputs) {
     inputs.tlVelocityRPM = m_leftVelSignal.refresh().getValueAsDouble() * 60.0;
     inputs.trVelocityRPM = m_rightVelSignal.refresh().getValueAsDouble() * 60.0;
 
+    Logger.recordOutput("Shooter/SysId/Left Vel RPS", m_leftVelSignal.refresh().getValueAsDouble());
+    Logger.recordOutput("Shooter/SysId/Right Vel RPS", m_rightVelSignal.refresh().getValueAsDouble());
+
     inputs.tlAppliedVolts = m_leftVoltOutSignal.getValueAsDouble();
     inputs.trAppliedVolts = m_rightVoltOutSignal.getValueAsDouble();
     inputs.kickerAppliedVolts = m_kickerVoltOutSignal.getValueAsDouble();

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java
@@ -2,16 +2,22 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Voltage;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import lib.utils.AimbotUtils;
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
+import static edu.wpi.first.units.Units.Volts;
+
 public class ShooterSubsystem extends SubsystemBase {
 
   private final ShooterIO m_io;
@@ -27,16 +33,27 @@ public class ShooterSubsystem extends SubsystemBase {
   private final LoggedDashboardNumber m_rightSetpoint =
       new LoggedDashboardNumber("Shooter/Right RPM", 4750);
 
+  private final SysIdRoutine m_sysId;
+
   public ShooterSubsystem(ShooterIO io) {
-    this(io, () -> new Pose2d());
+    this(io, Pose2d::new);
   }
 
   public ShooterSubsystem(ShooterIO io, Supplier<Pose2d> pose2dSupplier) {
     m_io = io;
     m_inputs = new ShooterIOInputsAutoLogged();
 
     m_poseSupplier = pose2dSupplier;
-    // turn on logging
+
+    m_sysId =
+            new SysIdRoutine(
+                    new SysIdRoutine.Config(
+                            null,
+                            null,
+                            null,
+                            (SysIdRoutineLog.State state) ->
+                                    Logger.recordOutput("Flywheel/SysIdState", state.toString())),
+                    new SysIdRoutine.Mechanism(voltage -> runCharacterizationVolts(voltage.in(Volts)), null, this));
   }
 
   @Override
@@ -65,6 +82,11 @@ public void setIntakePower(double power) {
     m_io.setIntakeVoltage(power * 12.0);
   }
 
+  public void runCharacterizationVolts(double volts) {
+    m_io.setMotorVoltageTL(volts);
+    m_io.setMotorVoltageTR(volts);
+  }
+
   public Command runShooterVelocity(boolean runKicker) {
     double distance = Units.metersToInches(AimbotUtils.getDistanceFromSpeaker(m_poseSupplier.get()));
     return runShooterVelocity(runKicker,
@@ -146,4 +168,14 @@ public boolean atSpeed() {
     return Math.abs(m_leftSpeedSetpoint - m_inputs.tlVelocityRPM) < 150
         && Math.abs(m_rightSpeedSetpoint - m_inputs.trVelocityRPM) < 150;
   }
+
+  /** Returns a command to run a quasistatic test in the specified direction. */
+  public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+    return m_sysId.quasistatic(direction);
+  }
+
+  /** Returns a command to run a dynamic test in the specified direction. */
+  public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+    return m_sysId.dynamic(direction);
+  }
 }