From 69f61d4f8d40821fac910998312662f6b8cb0716 Mon Sep 17 00:00:00 2001 From: Clulogh <197954988+Clulogh@users.noreply.github.com> Date: Sat, 28 Mar 2026 13:27:38 -0700 Subject: [PATCH 1/3] made a is extended method for intake deploy is extended for easy access in retry logic made a intake deployed method for easier inputs for the retry logic the actual retry logic is not done --- .../intake_deploy/IntakeDeploySubsystem.java | 10 ++++++++++ .../commands/IntakeDeployRetryCommand.java | 18 ++++++++++++++++++ 2 files changed, 28 insertions(+) create mode 100644 src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployRetryCommand.java diff --git a/src/main/java/competition/subsystems/intake_deploy/IntakeDeploySubsystem.java b/src/main/java/competition/subsystems/intake_deploy/IntakeDeploySubsystem.java index cac6bc96..d7c1e9f2 100644 --- a/src/main/java/competition/subsystems/intake_deploy/IntakeDeploySubsystem.java +++ b/src/main/java/competition/subsystems/intake_deploy/IntakeDeploySubsystem.java @@ -38,6 +38,8 @@ public class IntakeDeploySubsystem extends BaseSetpointSubsystem public final DoubleProperty maxPidAcceleration; public final DoubleProperty collectionDownwardPressure; public final DoubleProperty voltageRampTime; + public AngleProperty offset; + public DoubleProperty tolerance; public DoubleProperty readinessTimeoutSeconds; @Inject @@ -82,6 +84,7 @@ public IntakeDeploySubsystem(XCANMotorController.XCANMotorControllerFactory xcan this.retractedPosition = propertyFactory.createPersistentProperty("RetractedPosition", 10.0); this.extendedPosition = propertyFactory.createPersistentProperty("ExtendedPosition", -145.0); + this.offset = propertyFactory.createPersistentProperty("Offset from target", Degrees.of(3)); //TODO change to correct value this.manualControlPower = propertyFactory.createPersistentProperty("ManualControlPower", 0.2); @@ -194,4 +197,11 @@ public Current getMotorCurrent() { public Command getWaitForAtGoalCommand() { return new SimpleWaitForMaintainerCommand(this, () -> readinessTimeoutSeconds.get()); } + + public boolean intakeDeployIsExtended() { + var current = getCurrentValue(); + return current.isNear + (Degrees.of(extendedPosition.get()), + (Degrees.of(tolerance.get()))); + } } diff --git a/src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployRetryCommand.java b/src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployRetryCommand.java new file mode 100644 index 00000000..ee8ac1ec --- /dev/null +++ b/src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployRetryCommand.java @@ -0,0 +1,18 @@ +package competition.subsystems.intake_deploy.commands; + +import competition.subsystems.intake_deploy.IntakeDeploySubsystem; +import xbot.common.command.BaseCommand; + +public class IntakeDeployRetryCommand extends BaseCommand { + IntakeDeploySubsystem intakeDeploySubsystem; + + public IntakeDeployRetryCommand(IntakeDeploySubsystem intakeDeploy) { + this.intakeDeploySubsystem = intakeDeploy; + addRequirements(intakeDeploy); + } + + @Override + public boolean isFinished() { + return intakeDeploySubsystem.intakeDeployIsExtended(); + } +} From 78b8eded43a5ca7028878dfc973c949b0a626324 Mon Sep 17 00:00:00 2001 From: Clulogh <197954988+Clulogh@users.noreply.github.com> Date: Sat, 28 Mar 2026 14:30:17 -0700 Subject: [PATCH 2/3] Added intake deploy retry logic also added timeout --- .../commands/IntakeDeployRetryCommand.java | 20 ++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployRetryCommand.java b/src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployRetryCommand.java index ee8ac1ec..38ff2123 100644 --- a/src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployRetryCommand.java +++ b/src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployRetryCommand.java @@ -1,18 +1,32 @@ package competition.subsystems.intake_deploy.commands; import competition.subsystems.intake_deploy.IntakeDeploySubsystem; +import edu.wpi.first.wpilibj.Timer; +import org.json.Property; import xbot.common.command.BaseCommand; +import xbot.common.command.SimpleWaitForMaintainerCommand; +import xbot.common.properties.DoubleProperty; +import xbot.common.properties.PropertyFactory; public class IntakeDeployRetryCommand extends BaseCommand { IntakeDeploySubsystem intakeDeploySubsystem; + public DoubleProperty timeout; + public double startTime = 0; - public IntakeDeployRetryCommand(IntakeDeploySubsystem intakeDeploy) { + public IntakeDeployRetryCommand(IntakeDeploySubsystem intakeDeploy, PropertyFactory pf) { this.intakeDeploySubsystem = intakeDeploy; addRequirements(intakeDeploy); + this.timeout = pf.createPersistentProperty("SecondsInTimeout", 3.0); } + public boolean isTimeoutExpired() { + //refer to WimpleWaitForMaintainerCommand + return Timer.getFPGATimestamp() > startTime + timeout.get(); + } + + @Override public boolean isFinished() { - return intakeDeploySubsystem.intakeDeployIsExtended(); + return (isTimeoutExpired()) || intakeDeploySubsystem.intakeDeployIsExtended(); } -} +} \ No newline at end of file From 4995c5181c2989de7b2ad3a43a1ce29002747b47 Mon Sep 17 00:00:00 2001 From: Clulogh <197954988+Clulogh@users.noreply.github.com> Date: Sat, 28 Mar 2026 14:33:04 -0700 Subject: [PATCH 3/3] Removed unnecessary imports and removed comment --- .../intake_deploy/commands/IntakeDeployRetryCommand.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployRetryCommand.java b/src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployRetryCommand.java index 38ff2123..494fe72d 100644 --- a/src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployRetryCommand.java +++ b/src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployRetryCommand.java @@ -2,9 +2,7 @@ import competition.subsystems.intake_deploy.IntakeDeploySubsystem; import edu.wpi.first.wpilibj.Timer; -import org.json.Property; import xbot.common.command.BaseCommand; -import xbot.common.command.SimpleWaitForMaintainerCommand; import xbot.common.properties.DoubleProperty; import xbot.common.properties.PropertyFactory; @@ -20,7 +18,6 @@ public IntakeDeployRetryCommand(IntakeDeploySubsystem intakeDeploy, PropertyFact } public boolean isTimeoutExpired() { - //refer to WimpleWaitForMaintainerCommand return Timer.getFPGATimestamp() > startTime + timeout.get(); }