diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e8ba535d..88afc6ca 100644 --- 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 index 1e0eb54f..4aa23a1a 100644 --- 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 index ee9b8d4e..ee583646 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -2,9 +2,13 @@ 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; @@ -12,6 +16,8 @@ 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,8 +33,10 @@ 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 pose2dSupplier) { @@ -36,7 +44,16 @@ public ShooterSubsystem(ShooterIO io, Supplier pose2dSupplier) { 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); + } }