Convert toHeights to instant, add .until static methods

StuyPulse · Jan 30, 2024 · 672c5b9 · 672c5b9
1 parent 8542c21
commit 672c5b9
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 11 deletions.
diff --git 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
@@ -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
@@ -4,7 +4,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 
-public class IntakeAcquire extends Command{
+public class IntakeAcquire extends Command {
 
     private Intake intake;