diff --git a/src/main/java/frc/robot/constants/FunnelConstants.java b/src/main/java/frc/robot/constants/FunnelConstants.java index 9c2e1237..ce57da5b 100644 --- a/src/main/java/frc/robot/constants/FunnelConstants.java +++ b/src/main/java/frc/robot/constants/FunnelConstants.java @@ -12,9 +12,9 @@ import com.ctre.phoenix6.signals.ReverseLimitTypeValue; public class FunnelConstants { - public static final double kFunnelPercentOutput = 0; + public static final double kFunnelPercentOutput = 0.6; - public static int FunnelFxsId = 0; + public static int FunnelFxsId = 40; public static TalonFXSConfiguration getFXSConfig() { TalonFXSConfiguration fxsConfig = new TalonFXSConfiguration(); diff --git a/src/main/java/frc/robot/subsystems/funnel/FunnelIOFXS.java b/src/main/java/frc/robot/subsystems/funnel/FunnelIOFXS.java index 371ccc82..b5b7e9ea 100644 --- a/src/main/java/frc/robot/subsystems/funnel/FunnelIOFXS.java +++ b/src/main/java/frc/robot/subsystems/funnel/FunnelIOFXS.java @@ -9,7 +9,7 @@ import com.ctre.phoenix6.configs.TalonFXSConfigurator; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.hardware.TalonFXS; -import com.ctre.phoenix6.signals.ReverseLimitValue; +import com.ctre.phoenix6.signals.ForwardLimitValue; import edu.wpi.first.units.measure.AngularVelocity; import frc.robot.constants.FunnelConstants; @@ -22,7 +22,7 @@ public class FunnelIOFXS implements FunnelIo{ // FX Acces Objects TalonFXSConfigurator configurator; StatusSignal curVelocity; - StatusSignal curRevLimit; + StatusSignal curFwdLimit; private DutyCycleOut dutyCycleRequest = new DutyCycleOut(0.0).withEnableFOC(false); @@ -37,7 +37,7 @@ public FunnelIOFXS() { // Attach status signals curVelocity = talonfxs.getVelocity(); - curRevLimit = talonfxs.getReverseLimit(); + curFwdLimit = talonfxs.getForwardLimit(); } @Override @@ -48,11 +48,21 @@ public void setPct(double percentOutput){ @Override public void updateInputs(FunnelIoInputs inputs) { inputs.velocity = curVelocity.refresh().getValue(); - inputs.revBeamOpen = curRevLimit.refresh().getValue().value == 1; + inputs.fwdBeamOpen = curFwdLimit.refresh().getValue().value == 1; } @Override public void registerWith(TelemetryService telemetryService) { telemetryService.register(talonfxs, true); } + + @Override + public void enableFwdLimitSwitch(boolean enabled) { + talonfxs + .getConfigurator() + .apply( + FunnelConstants.getFXSConfig() + .HardwareLimitSwitch + .withForwardLimitEnable(enabled)); + } } diff --git a/src/main/java/frc/robot/subsystems/funnel/FunnelIo.java b/src/main/java/frc/robot/subsystems/funnel/FunnelIo.java index 6c5ab31a..1a6159f3 100644 --- a/src/main/java/frc/robot/subsystems/funnel/FunnelIo.java +++ b/src/main/java/frc/robot/subsystems/funnel/FunnelIo.java @@ -11,9 +11,12 @@ public interface FunnelIo { public static class FunnelIoInputs{ public AngularVelocity velocity; // Open = beam isnt broken - public boolean revBeamOpen = false; + public boolean fwdBeamOpen = false; + public boolean beamEnabled = true; } + public default void enableFwdLimitSwitch(boolean enabled) {} + public default void setPct(double percentOutput) {} public default void updateInputs(FunnelIoInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/funnel/FunnelSubsystem.java b/src/main/java/frc/robot/subsystems/funnel/FunnelSubsystem.java index 1a8e259f..20778298 100644 --- a/src/main/java/frc/robot/subsystems/funnel/FunnelSubsystem.java +++ b/src/main/java/frc/robot/subsystems/funnel/FunnelSubsystem.java @@ -9,7 +9,6 @@ import frc.robot.constants.FunnelConstants; import frc.robot.standards.OpenLoopSubsystem; -import frc.robot.subsystems.funnel.FunnelIOInputsAutoLogged; public class FunnelSubsystem extends MeasurableSubsystem implements OpenLoopSubsystem{ @@ -30,6 +29,11 @@ public FunnelState getState() { return curState; } + public boolean hasCoral() { + // return true; + return curState == FunnelState.HasSeenCoral; + } + @Override public void periodic() { // Update Inputs @@ -40,15 +44,16 @@ public void periodic() { case HasSeenCoral: break; case HasNotSeenCoral: - if(inputs.revBeamOpen == false){ - totalBreaks += 1; - }else{ - totalBreaks = 0; - } - - if(totalBreaks >= 3){ - curState = FunnelState.HasSeenCoral; - } + if(inputs.fwdBeamOpen == false){ + totalBreaks += 1; + }else{ + totalBreaks = 0; + } + + if(totalBreaks >= 3){ + stopMotor(); + curState = FunnelState.HasSeenCoral; + } break; } @@ -80,16 +85,19 @@ public void setPercent(double pct) { io.setPct(pct); } - public void StartMotor() { + public void startMotor() { setPercent(FunnelConstants.kFunnelPercentOutput); - } - - public void StopMotor() { - setPercent(0.0); + io.enableFwdLimitSwitch(false); } public void ClearCoral() { curState = FunnelState.HasNotSeenCoral; + setPercent(FunnelConstants.kFunnelPercentOutput); + io.enableFwdLimitSwitch(true); + } + + public void stopMotor() { + setPercent(0.0); + io.enableFwdLimitSwitch(false); } - }