From aab9492702134b5a6afd102cc93f8860a8375633 Mon Sep 17 00:00:00 2001 From: BB7209 Date: Thu, 6 Feb 2025 19:19:00 -0700 Subject: [PATCH 1/5] added fxs ID --- src/main/java/frc/robot/constants/FunnelConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/constants/FunnelConstants.java b/src/main/java/frc/robot/constants/FunnelConstants.java index 9c2e1237..3251aed9 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(); From c44d0ea3fe77d42f3f1fba84774f442a4a382be8 Mon Sep 17 00:00:00 2001 From: BB7209 Date: Tue, 11 Feb 2025 18:40:16 -0700 Subject: [PATCH 2/5] Start of logic changes --- src/main/java/frc/robot/constants/FunnelConstants.java | 2 +- .../java/frc/robot/subsystems/funnel/FunnelIOFXS.java | 8 ++++---- .../java/frc/robot/subsystems/funnel/FunnelIo.java | 3 ++- .../frc/robot/subsystems/funnel/FunnelSubsystem.java | 10 +++++++--- 4 files changed, 14 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/constants/FunnelConstants.java b/src/main/java/frc/robot/constants/FunnelConstants.java index 3251aed9..ce57da5b 100644 --- a/src/main/java/frc/robot/constants/FunnelConstants.java +++ b/src/main/java/frc/robot/constants/FunnelConstants.java @@ -12,7 +12,7 @@ import com.ctre.phoenix6.signals.ReverseLimitTypeValue; public class FunnelConstants { - public static final double kFunnelPercentOutput = -0.6; + public static final double kFunnelPercentOutput = 0.6; public static int FunnelFxsId = 40; diff --git a/src/main/java/frc/robot/subsystems/funnel/FunnelIOFXS.java b/src/main/java/frc/robot/subsystems/funnel/FunnelIOFXS.java index 371ccc82..69ada12f 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 curForLimit; private DutyCycleOut dutyCycleRequest = new DutyCycleOut(0.0).withEnableFOC(false); @@ -37,7 +37,7 @@ public FunnelIOFXS() { // Attach status signals curVelocity = talonfxs.getVelocity(); - curRevLimit = talonfxs.getReverseLimit(); + curForLimit = talonfxs.getForwardLimit(); } @Override @@ -48,7 +48,7 @@ public void setPct(double percentOutput){ @Override public void updateInputs(FunnelIoInputs inputs) { inputs.velocity = curVelocity.refresh().getValue(); - inputs.revBeamOpen = curRevLimit.refresh().getValue().value == 1; + inputs.forBeamOpen = curForLimit.refresh().getValue().value == 1; } @Override diff --git a/src/main/java/frc/robot/subsystems/funnel/FunnelIo.java b/src/main/java/frc/robot/subsystems/funnel/FunnelIo.java index 6c5ab31a..10a4e4e2 100644 --- a/src/main/java/frc/robot/subsystems/funnel/FunnelIo.java +++ b/src/main/java/frc/robot/subsystems/funnel/FunnelIo.java @@ -11,7 +11,8 @@ public interface FunnelIo { public static class FunnelIoInputs{ public AngularVelocity velocity; // Open = beam isnt broken - public boolean revBeamOpen = false; + public boolean forBeamOpen = false; + public boolean beamEnabled = true; } public default void setPct(double percentOutput) {} diff --git a/src/main/java/frc/robot/subsystems/funnel/FunnelSubsystem.java b/src/main/java/frc/robot/subsystems/funnel/FunnelSubsystem.java index 1a8e259f..2e8b49f7 100644 --- a/src/main/java/frc/robot/subsystems/funnel/FunnelSubsystem.java +++ b/src/main/java/frc/robot/subsystems/funnel/FunnelSubsystem.java @@ -40,7 +40,7 @@ public void periodic() { case HasSeenCoral: break; case HasNotSeenCoral: - if(inputs.revBeamOpen == false){ + if(inputs.forBeamOpen == false){ totalBreaks += 1; }else{ totalBreaks = 0; @@ -80,10 +80,14 @@ public void setPercent(double pct) { io.setPct(pct); } - public void StartMotor() { - setPercent(FunnelConstants.kFunnelPercentOutput); + public void PassCoral() { + } + public void StopCoral() { + + } + public void StopMotor() { setPercent(0.0); } From 883f373a7d6fb664f2873a1cee6da12f10abeaba Mon Sep 17 00:00:00 2001 From: BB7209 Date: Thu, 13 Feb 2025 18:45:01 -0700 Subject: [PATCH 3/5] i dont think this is right --- .../frc/robot/subsystems/funnel/FunnelIOFXS.java | 16 +++++++++++++--- .../frc/robot/subsystems/funnel/FunnelIo.java | 6 +++++- .../robot/subsystems/funnel/FunnelSubsystem.java | 15 ++++++++++++--- 3 files changed, 30 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/funnel/FunnelIOFXS.java b/src/main/java/frc/robot/subsystems/funnel/FunnelIOFXS.java index 69ada12f..b5b7e9ea 100644 --- a/src/main/java/frc/robot/subsystems/funnel/FunnelIOFXS.java +++ b/src/main/java/frc/robot/subsystems/funnel/FunnelIOFXS.java @@ -22,7 +22,7 @@ public class FunnelIOFXS implements FunnelIo{ // FX Acces Objects TalonFXSConfigurator configurator; StatusSignal curVelocity; - StatusSignal curForLimit; + StatusSignal curFwdLimit; private DutyCycleOut dutyCycleRequest = new DutyCycleOut(0.0).withEnableFOC(false); @@ -37,7 +37,7 @@ public FunnelIOFXS() { // Attach status signals curVelocity = talonfxs.getVelocity(); - curForLimit = talonfxs.getForwardLimit(); + 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.forBeamOpen = curForLimit.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 10a4e4e2..728087f7 100644 --- a/src/main/java/frc/robot/subsystems/funnel/FunnelIo.java +++ b/src/main/java/frc/robot/subsystems/funnel/FunnelIo.java @@ -3,6 +3,8 @@ import org.littletonrobotics.junction.AutoLog; import org.strykeforce.telemetry.TelemetryService; +import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; + import edu.wpi.first.units.measure.AngularVelocity; public interface FunnelIo { @@ -11,10 +13,12 @@ public interface FunnelIo { public static class FunnelIoInputs{ public AngularVelocity velocity; // Open = beam isnt broken - public boolean forBeamOpen = 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 2e8b49f7..05f177ec 100644 --- a/src/main/java/frc/robot/subsystems/funnel/FunnelSubsystem.java +++ b/src/main/java/frc/robot/subsystems/funnel/FunnelSubsystem.java @@ -38,9 +38,16 @@ public void periodic() { switch (curState){ case HasSeenCoral: + if(inputs.fwdBeamOpen == false) { + setPercent(0.0); + }else{ + setPercent(FunnelConstants.kFunnelPercentOutput); + } break; case HasNotSeenCoral: - if(inputs.forBeamOpen == false){ + setPercent(FunnelConstants.kFunnelPercentOutput); + + if(inputs.fwdBeamOpen == false){ totalBreaks += 1; }else{ totalBreaks = 0; @@ -81,11 +88,13 @@ public void setPercent(double pct) { } public void PassCoral() { - + io.enableFwdLimitSwitch(false); + setPercent(FunnelConstants.kFunnelPercentOutput); } public void StopCoral() { - + io.enableFwdLimitSwitch(true); + setPercent(0.0); } public void StopMotor() { From 5a32c709984043300b124628cdeabb47dc38f233 Mon Sep 17 00:00:00 2001 From: BB7209 Date: Tue, 18 Feb 2025 18:23:36 -0700 Subject: [PATCH 4/5] I think it works --- .../frc/robot/subsystems/funnel/FunnelIo.java | 2 - .../subsystems/funnel/FunnelSubsystem.java | 44 +++++++------------ 2 files changed, 16 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/funnel/FunnelIo.java b/src/main/java/frc/robot/subsystems/funnel/FunnelIo.java index 728087f7..1a6159f3 100644 --- a/src/main/java/frc/robot/subsystems/funnel/FunnelIo.java +++ b/src/main/java/frc/robot/subsystems/funnel/FunnelIo.java @@ -3,8 +3,6 @@ import org.littletonrobotics.junction.AutoLog; import org.strykeforce.telemetry.TelemetryService; -import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; - import edu.wpi.first.units.measure.AngularVelocity; public interface FunnelIo { diff --git a/src/main/java/frc/robot/subsystems/funnel/FunnelSubsystem.java b/src/main/java/frc/robot/subsystems/funnel/FunnelSubsystem.java index 05f177ec..452d5af2 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{ @@ -38,24 +37,18 @@ public void periodic() { switch (curState){ case HasSeenCoral: - if(inputs.fwdBeamOpen == false) { - setPercent(0.0); - }else{ - setPercent(FunnelConstants.kFunnelPercentOutput); - } break; case HasNotSeenCoral: - setPercent(FunnelConstants.kFunnelPercentOutput); - - if(inputs.fwdBeamOpen == false){ - totalBreaks += 1; - }else{ - totalBreaks = 0; - } - - if(totalBreaks >= 3){ - curState = FunnelState.HasSeenCoral; - } + if(inputs.fwdBeamOpen == false){ + totalBreaks += 1; + }else{ + totalBreaks = 0; + } + + if(totalBreaks >= 3){ + setPercent(0.0); + curState = FunnelState.HasSeenCoral; + } break; } @@ -87,22 +80,17 @@ public void setPercent(double pct) { io.setPct(pct); } - public void PassCoral() { - io.enableFwdLimitSwitch(false); - setPercent(FunnelConstants.kFunnelPercentOutput); + public void EnableBeamBreak() { + io.enableFwdLimitSwitch(true); + curState = FunnelState.HasNotSeenCoral; } - public void StopCoral() { - io.enableFwdLimitSwitch(true); - setPercent(0.0); + public void DisableBeamBreak() { + io.enableFwdLimitSwitch(false); + setPercent(FunnelConstants.kFunnelPercentOutput); } public void StopMotor() { setPercent(0.0); } - - public void ClearCoral() { - curState = FunnelState.HasNotSeenCoral; - } - } From 1c470542e529d2e756c1f2c9784bef5017cb92e8 Mon Sep 17 00:00:00 2001 From: BB7209 Date: Tue, 18 Feb 2025 18:49:32 -0700 Subject: [PATCH 5/5] fixed names --- .../subsystems/funnel/FunnelSubsystem.java | 21 ++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/funnel/FunnelSubsystem.java b/src/main/java/frc/robot/subsystems/funnel/FunnelSubsystem.java index 452d5af2..20778298 100644 --- a/src/main/java/frc/robot/subsystems/funnel/FunnelSubsystem.java +++ b/src/main/java/frc/robot/subsystems/funnel/FunnelSubsystem.java @@ -29,6 +29,11 @@ public FunnelState getState() { return curState; } + public boolean hasCoral() { + // return true; + return curState == FunnelState.HasSeenCoral; + } + @Override public void periodic() { // Update Inputs @@ -46,7 +51,7 @@ public void periodic() { } if(totalBreaks >= 3){ - setPercent(0.0); + stopMotor(); curState = FunnelState.HasSeenCoral; } break; @@ -80,17 +85,19 @@ public void setPercent(double pct) { io.setPct(pct); } - public void EnableBeamBreak() { - io.enableFwdLimitSwitch(true); - curState = FunnelState.HasNotSeenCoral; + public void startMotor() { + setPercent(FunnelConstants.kFunnelPercentOutput); + io.enableFwdLimitSwitch(false); } - public void DisableBeamBreak() { - io.enableFwdLimitSwitch(false); + public void ClearCoral() { + curState = FunnelState.HasNotSeenCoral; setPercent(FunnelConstants.kFunnelPercentOutput); + io.enableFwdLimitSwitch(true); } - public void StopMotor() { + public void stopMotor() { setPercent(0.0); + io.enableFwdLimitSwitch(false); } }