diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index cc665a1a..fa981fdf 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -10,6 +10,7 @@ public static class ShooterIOInputs { public double velocityLeft; public double velocityRight; public double leftSetpoint = 0.0; + public boolean isFwdLimitSwitchClosed = false; } public default void updateInputs(ShooterIOInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOFX.java index f8eecc72..3efe612e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOFX.java @@ -7,6 +7,10 @@ import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.ForwardLimitSourceValue; +import com.ctre.phoenix6.signals.ForwardLimitValue; + +import frc.robot.constants.MagazineConstants; import frc.robot.constants.ShooterConstants; import org.slf4j.Logger; import org.slf4j.LoggerFactory; @@ -50,6 +54,8 @@ public boolean followTalons() { StatusSignal curLeftVelocity; StatusSignal curRightVelocity; + private StatusSignal fwdLimitSwitch; + public ShooterIOFX() { logger = LoggerFactory.getLogger(this.getClass()); shooterLeft = new TalonFX(ShooterConstants.kLeftShooterTalonID); @@ -57,7 +63,11 @@ public ShooterIOFX() { configurator = shooterLeft.getConfigurator(); configurator.apply(new TalonFXConfiguration()); // factory default - configurator.apply(ShooterConstants.getLeftShooterConfig()); + configurator.apply( + ShooterConstants.getLeftShooterConfig() + .HardwareLimitSwitch + .withForwardLimitSource(ForwardLimitSourceValue.RemoteTalonFX) + .withForwardLimitRemoteSensorID(MagazineConstants.kMagazineFalconID)); configurator = shooterRight.getConfigurator(); configurator.apply(new TalonFXConfiguration()); // factory default @@ -65,6 +75,8 @@ public ShooterIOFX() { curLeftVelocity = shooterLeft.getVelocity(); curRightVelocity = shooterRight.getVelocity(); + + fwdLimitSwitch = shooterLeft.getForwardLimit(); } @Override @@ -77,6 +89,7 @@ public void updateInputs(ShooterIOInputs inputs) { inputs.leftSetpoint = leftSetpoint; inputs.velocityLeft = curLeftVelocity.refresh().getValue(); inputs.velocityRight = curRightVelocity.refresh().getValue(); + inputs.isFwdLimitSwitchClosed = fwdLimitSwitch.refresh().getValue().value == 1; } @Override diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 51723eba..23f19738 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -80,6 +80,7 @@ public void setPercent(double pct) { pctOut = true; io.setPct(pct); } + // Periodic @Override public void periodic() {