added AmperWaitToHeight + cleaned command code

src/main/java/com/stuypulse/robot/commands/amper/AmperIntake.java

@@ -6,8 +6,7 @@
 
 public class AmperIntake extends Command {
 
-    public Amper amper;
-    public boolean forward;
+    private final Amper amper;
 
     public AmperIntake() {
         amper = Amper.getInstance();

src/main/java/com/stuypulse/robot/commands/amper/AmperOuttake.java

@@ -6,8 +6,7 @@
 
 public class AmperOuttake extends Command {
 
-    public Amper amper;
-    public boolean forward;
+    private final Amper amper;
 
     public AmperOuttake() {
         amper = Amper.getInstance();

src/main/java/com/stuypulse/robot/commands/amper/AmperScoreAmp.java

@@ -1,12 +1,15 @@
 package com.stuypulse.robot.commands.amper;
 
 import com.stuypulse.robot.constants.Settings.Amper.Score;
+
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 
 public class AmperScoreAmp extends SequentialCommandGroup {
+
     public AmperScoreAmp() {
         addCommands(
             new AmperToHeight(Score.AMP_SCORE_HEIGHT.get()),
+            new AmperWaitToHeight(Score.AMP_SCORE_HEIGHT.get()),
             new AmperOuttake()
         );
     }

src/main/java/com/stuypulse/robot/commands/amper/AmperScoreTrap.java

@@ -9,6 +9,7 @@ public class AmperScoreTrap extends SequentialCommandGroup {
     public AmperScoreTrap() {
         addCommands(
             new AmperToHeight(Score.TRAP_SCORE_HEIGHT.get()),
+            new AmperWaitToHeight(Score.TRAP_SCORE_HEIGHT.get()),
             new AmperOuttake()
         ); 
     }

src/main/java/com/stuypulse/robot/commands/amper/AmperToHeight.java

@@ -6,7 +6,7 @@
 
 public class AmperToHeight extends InstantCommand {
     private final Amper amper;
-    private double height;
+    private final double height;
 
     public AmperToHeight(double height) {
         amper = Amper.getInstance();

src/main/java/com/stuypulse/robot/commands/amper/AmperWaitToHeight.java

@@ -0,0 +1,22 @@
+package com.stuypulse.robot.commands.amper;
+
+import com.stuypulse.robot.constants.Settings.Amper.Lift;
+import com.stuypulse.robot.subsystems.amper.Amper;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class AmperWaitToHeight extends Command {
+
+    private final Amper amper;
+    private final double height;
+
+    public AmperWaitToHeight(double height) {
+        amper = Amper.getInstance();
+        this.height = height;
+    }
+
+    @Override
+    public boolean isFinished() {
+        return Math.abs(amper.getLiftHeight() - height) < Lift.MAX_HEIGHT_ERROR;
+    }
+}

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -43,8 +43,6 @@ public interface Encoder {
     }  
 
     public interface Amper {
-        SmartNumber REST_HEIGHT = new SmartNumber("Amper/Rest Height", 0.5); // TODO: determine
-
         public interface Score {
             SmartNumber ROLLER_SPEED = new SmartNumber("Amper/Score/Roller Speed", 1.0);
 
@@ -59,6 +57,7 @@ public interface Lift {
 
             double MIN_HEIGHT = 0;
             double MAX_HEIGHT = 1.8475325; // meters 
+            double REST_HEIGHT = 0.5;
 
             double MAX_HEIGHT_ERROR = 0.03;
 

src/main/java/com/stuypulse/robot/subsystems/amper/Amper.java

@@ -70,7 +70,7 @@ public Amper() {
         ));
         lift2d = mechanism2d.getRoot("Lift Origin", 1.5, 1).append(new MechanismLigament2d(
             "Lift",
-            Settings.Amper.Lift.MIN_HEIGHT,
+            Settings.Amper.Lift.REST_HEIGHT,
             Settings.Amper.Lift.ANGLE_TO_GROUND.getDegrees(),
             10,
             new Color8Bit(Color.kAqua)