diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index d7e8184..5f986c5 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -24,32 +24,36 @@ public class Intake extends DisableSubsystem { // note for me later - this has also controls the redirect roller private final SingleMotorSubsystemIO intakeIO; - private final SingleMotorSubsystemInputsAutoLogged intakeIOAutoLogged = new SingleMotorSubsystemInputsAutoLogged(); + private final SingleMotorSubsystemInputsAutoLogged intakeIOAutoLogged = + new SingleMotorSubsystemInputsAutoLogged(); private final SysIdRoutine intake_sysIdRoutine; private final Trigger debouncedBeamBreak = new Trigger(this::isBeamBroken).debounce(0.1); private final BeamBreakIO beamBreakIO; - private final BeamBreakIOInputsAutoLogged beamBreakIOAutoLogged = new BeamBreakIOInputsAutoLogged(); + private final BeamBreakIOInputsAutoLogged beamBreakIOAutoLogged = + new BeamBreakIOInputsAutoLogged(); public Intake(boolean disabled, SingleMotorSubsystemIO intakeIO, BeamBreakIO beamBreakIO) { super(disabled); this.intakeIO = intakeIO; this.beamBreakIO = beamBreakIO; - intake_sysIdRoutine = new SysIdRoutine( - new SysIdRoutine.Config( - Volts.of(0.2).per(Seconds.of(1)), // Use default ramp rate (1 V/s) - Volts.of(6), // Reduce dynamic step voltage to 4 to prevent brownout - null, // Use default timeout (10 s) - // Log state with Phoenix SignalLogger class - (state) -> SignalLogger.writeString("state", state.toString())), - new SysIdRoutine.Mechanism( - (volts) -> intakeIO - .getMotor() - .setControl(intakeIO.getVoltageRequest().withOutput(volts.in(Volts))), - null, - this)); + intake_sysIdRoutine = + new SysIdRoutine( + new SysIdRoutine.Config( + Volts.of(0.2).per(Seconds.of(1)), // Use default ramp rate (1 V/s) + Volts.of(6), // Reduce dynamic step voltage to 4 to prevent brownout + null, // Use default timeout (10 s) + // Log state with Phoenix SignalLogger class + (state) -> SignalLogger.writeString("state", state.toString())), + new SysIdRoutine.Mechanism( + (volts) -> + intakeIO + .getMotor() + .setControl(intakeIO.getVoltageRequest().withOutput(volts.in(Volts))), + null, + this)); } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index a8349b4..92bff12 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -15,21 +15,22 @@ public final class IntakeConstants implements SingleMotorConstants { // Motor configuration public static final int kMotorID = 33; - public static final TalonFXConfiguration kMotorConfig = new TalonFXConfiguration() - .withSlot0(new Slot0Configs().withKS(0).withKV(0.1).withKP(1).withKI(0).withKD(0)) - .withMotorOutput( - new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Brake) - .withInverted(InvertedValue.Clockwise_Positive)) - .withMotionMagic( - new MotionMagicConfigs() - .withMotionMagicAcceleration(120) - .withMotionMagicCruiseVelocity(60) - .withMotionMagicJerk(1200)) - .withCurrentLimits( - new CurrentLimitsConfigs() - .withStatorCurrentLimitEnable(true) - .withStatorCurrentLimit(80)); + public static final TalonFXConfiguration kMotorConfig = + new TalonFXConfiguration() + .withSlot0(new Slot0Configs().withKS(0).withKV(0.1).withKP(1).withKI(0).withKD(0)) + .withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withInverted(InvertedValue.Clockwise_Positive)) + .withMotionMagic( + new MotionMagicConfigs() + .withMotionMagicAcceleration(120) + .withMotionMagicCruiseVelocity(60) + .withMotionMagicJerk(1200)) + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimitEnable(true) + .withStatorCurrentLimit(80)); public static final boolean kUseMotionMagic = false; // Custom to intake subsystem public static final double kMotorVoltage = 12;