Intake deploy retry logic

src/main/java/competition/subsystems/intake_deploy/IntakeDeploySubsystem.java

@@ -38,6 +38,8 @@ public class IntakeDeploySubsystem extends BaseSetpointSubsystem<Angle,Double>
     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())));
+    }
 }

src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployRetryCommand.java

@@ -0,0 +1,29 @@
+package competition.subsystems.intake_deploy.commands;
+
+import competition.subsystems.intake_deploy.IntakeDeploySubsystem;
+import edu.wpi.first.wpilibj.Timer;
+import xbot.common.command.BaseCommand;
+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, PropertyFactory pf) {
+        this.intakeDeploySubsystem = intakeDeploy;
+        addRequirements(intakeDeploy);
+        this.timeout = pf.createPersistentProperty("SecondsInTimeout", 3.0);
+    }
+
+    public boolean isTimeoutExpired() {
+        return Timer.getFPGATimestamp() > startTime + timeout.get();
+    }
+
+
+    @Override
+    public boolean isFinished() {
+        return (isTimeoutExpired()) || intakeDeploySubsystem.intakeDeployIsExtended();
+    }
+}