diff --git a/src/main/java/com/stuypulse/robot/commands/amper/AmperToHeight.java b/src/main/java/com/stuypulse/robot/commands/amper/AmperToHeight.java index 42e56cff..c3ca547c 100644 --- a/src/main/java/com/stuypulse/robot/commands/amper/AmperToHeight.java +++ b/src/main/java/com/stuypulse/robot/commands/amper/AmperToHeight.java @@ -4,8 +4,14 @@ import com.stuypulse.robot.subsystems.amper.Amper; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; -public class AmperToHeight extends Command { +public class AmperToHeight extends InstantCommand { + + public static Command untilDone(double height) { + return new AmperToHeight(height) + .until(() -> Math.abs(Amper.getInstance().getLiftHeight() - height) < Lift.MAX_HEIGHT_ERROR); + } private final Amper amper; private final double height; @@ -22,8 +28,4 @@ public void initialize() { amper.setTargetHeight(height); } - @Override - public boolean isFinished() { - return Math.abs(amper.getLiftHeight() - height) < Lift.MAX_HEIGHT_ERROR; - } } diff --git a/src/main/java/com/stuypulse/robot/commands/climber/ClimberToHeight.java b/src/main/java/com/stuypulse/robot/commands/climber/ClimberToHeight.java index 320dabea..68fb83e0 100644 --- a/src/main/java/com/stuypulse/robot/commands/climber/ClimberToHeight.java +++ b/src/main/java/com/stuypulse/robot/commands/climber/ClimberToHeight.java @@ -4,8 +4,16 @@ import com.stuypulse.robot.subsystems.climber.Climber; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; -public class ClimberToHeight extends Command { +public class ClimberToHeight extends InstantCommand { + + public static Command untilDone(double height) { + Climber climber = Climber.getInstance(); + + return new ClimberToHeight(height) + .until(() -> Math.abs(climber.getTargetHeight() - climber.getHeight()) < Settings.Climber.BangBang.THRESHOLD); + } private final Climber climber; private final double height; @@ -22,8 +30,4 @@ public void initialize() { climber.setTargetHeight(height); } - @Override - public boolean isFinished() { - return Math.abs(climber.getTargetHeight() - climber.getHeight()) < Settings.Climber.BangBang.THRESHOLD; - } } diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java index d57f1738..d88e4b90 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java @@ -4,7 +4,7 @@ import edu.wpi.first.wpilibj2.command.Command; -public class IntakeAcquire extends Command{ +public class IntakeAcquire extends Command { private Intake intake;