remove elevatorAt & armAt

Author: Saturn-VI

src/main/java/frc/robot/subsystems/lift/LiftInstructions.java

@@ -783,6 +783,8 @@ public record IDF (
    * @return The instruction set representing the transition from startingState to endingState. If there is no valid transition, return null
    */
   public static IDF[] mapStatesToTransition(TargetLiftStates startingState, TargetLiftStates endingState) {
+    // shortcut identical case
+    if (startingState == endingState) return null;
     switch (startingState) {
         case STOW:
             switch (endingState) {

src/main/java/frc/robot/subsystems/lift/LiftSubsystem.java

@@ -192,12 +192,7 @@ public SystemState nextState() {
     AT_STATE {
       @Override
       public SystemState nextState() {
-        // would technically already return due to mapStatesToTransition returning null,
-        // but this saves some time
-        if (curState == nextState) {
-          return this;
-        }
-
+        // mapStatesToTransition returns null if identical
         IDF[] idf = LiftInstructions.mapStatesToTransition(curState, nextState);
         if (idf != null) {
           currentInstructionSet = idf;
@@ -217,7 +212,8 @@ public void initialize() {
 
       @Override
       public void execute() {
-        if (s_liftinstance.armAt(STOW_ANGLE) && s_liftinstance.elevatorAt(STOW_HEIGHT)) {
+        if (s_liftinstance.getArmAngle().isNear(STOW_ANGLE, ARM_TOLERANCE)
+         && s_liftinstance.getElevatorHeight().isNear(STOW_HEIGHT, ELEVATOR_TOLERANCE)) {
           isLiftReady = true;
         } else {
           isLiftReady = false;
@@ -483,26 +479,6 @@ public boolean elevatorAtHome() {
     return elevatorHomingBeamBreak();
   }
 
-  /**
-   * Return whether the elevator is at a target height or not
-   *
-   * @return Boolean of if elevator is at target height
-   */
-  public boolean elevatorAt(Distance targetHeight) {
-    Distance currentHeight = getElevatorHeight();
-    return (currentHeight.isNear(targetHeight, ELEVATOR_TOLERANCE));
-  }
-
-  /**
-   * Return whether the arm is at a target angle or not
-   *
-   * @return Boolean of if arm is at target angle
-   */
-  public boolean armAt(Angle targetAngle) {
-    Angle currentAngle = getArmAngle();
-    return (currentAngle.isNear(targetAngle, ARM_TOLERANCE));
-  }
-
   /**
    * Return whether the lift is ready to move or not
    * @return Boolean of if lift is at one of the 5 stages