Adjusting moveManipulator so it's not so...rough

src/main/java/frc/robot/RobotContainer.java

@@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 
@@ -184,25 +185,41 @@ public class RobotContainer {
 	}
 
 	private Command moveManipulator(double elevatorPosition, double armPosition) {
-		if ((elevator.getEncoderPosition() > ElevatorConstants.kElevatorBracePosition && elevatorPosition > ElevatorConstants.kElevatorBracePosition) || (arm.getEncoderPosition() > ArmConstants.kArmSafeStowPosition && armPosition > ArmConstants.kArmSafeStowPosition)) { //if the elevator and target are above the brace, or the arm and target is in front of the brace, move together
+		// If the elevator current and target positions are above the brace, or the arm current and target position is in
-			return arm.goToSetpoint(armPosition, 2)
+		// front of the brace, move together
-				.alongWith(elevator.goToSetpoint(elevatorPosition, 2));
+		if ((elevator.isMotionSafe() && elevator.isMotionSafe(elevatorPosition)) || (arm.isMotionSafe() && arm.isMotionSafe(armPosition))) { 
-		} else if (armPosition < ArmConstants.kArmSafeStowPosition) { //if the target is behind the brace, move the elevator first
+			return moveManipulatorUtil(elevatorPosition, armPosition, false, false);
-			if (arm.getEncoderPosition() < ArmConstants.kArmSafeStowPosition) { //if the arm is behind the brace at start, move the arm to a safe position, then elevator, then arm
+		// If the target position is behind the brace, and the arm is not behind the brace, move the arm to a safe position first,
-				return arm.goToSetpoint(ArmConstants.kArmSafeStowPosition, 2)
+		// then the elevator, then the arm again
-					.andThen(elevator.goToSetpoint(elevatorPosition, 2))
+		} else if (!arm.isMotionSafe(armPosition) && !arm.isMotionSafe()) { 
-						.andThen(arm.goToSetpoint(armPosition, 2));
+			return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
-			} else { //if the arm is in front of the brace, move elevator, then arm
+				.andThen(arm.goToSetpoint(armPosition, 2));
-				return elevator.goToSetpoint(elevatorPosition, 2)
+		// If the target position is behind the brace, and the arm is behind the brace, move the elevator first, then the arm
-					.andThen(arm.goToSetpoint(armPosition, 2));
+		} else if (!arm.isMotionSafe(armPosition) && arm.isMotionSafe()) {
-			}
+			return moveManipulatorUtil(elevatorPosition, armPosition, true, true);
-		} else if (arm.getEncoderPosition() < ArmConstants.kArmSafeStowPosition) { //if the arm is behind the brace, move to target, then move the elevator
+		// If the arm is behind the brace, move the arm first, then the elevator
-			return arm.goToSetpoint(armPosition, 2)
+		} else if (!arm.isMotionSafe()) {
-				.andThen(elevator.goToSetpoint(elevatorPosition, 2));
+			return moveManipulatorUtil(elevatorPosition, armPosition, false, true);
-		} else { //catch all that will always safely move the arm in a safe way
+		// Catch all command that's safe regardless of arm and elevator positions
-			return arm.goToSetpoint(ArmConstants.kArmSafeStowPosition, 2)
+		} else { 
-				.andThen(elevator.goToSetpoint(elevatorPosition, 2))
+			return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
-					.andThen(arm.goToSetpoint(armPosition, 2));
+				.andThen(arm.goToSetpoint(armPosition, 2));
 		}
 	}
+
+	private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
+		return Commands.either(
+			Commands.either(
+				elevator.goToSetpoint(elevatorPosition, 2).andThen(arm.goToSetpoint(armPosition, 2)),
+				elevator.goToSetpoint(elevatorPosition, 2).alongWith(arm.goToSetpoint(armPosition, 2)),
+				() -> sequential
+			), 
+			Commands.either(
+				arm.goToSetpoint(armPosition, 2).andThen(elevator.goToSetpoint(elevatorPosition, 2)), 
+				arm.goToSetpoint(armPosition, 2).alongWith(elevator.goToSetpoint(elevatorPosition, 2)), 
+				() -> sequential
+			), 
+			() -> elevatorFirst
+		);
+	}
 }

src/main/java/frc/robot/subsystems/Arm.java

@@ -31,6 +31,27 @@ public class Arm extends SubsystemBase {
         canCoder = new CANcoder(ArmConstants.kCANcoderID);
     }
 
+    /**
+     * Returns whether or not the motion is safe relative to the encoder's current position
+     * and the arm safe stow position
+     * 
+     * @return Is the motion safe
+     */
+    public boolean isMotionSafe() {
+        return isMotionSafe(getEncoderPosition());
+    }
+
+    /**
+     * Returns whether or not the motion is safe relative to some target position and the
+     * arm safe stow position
+     * 
+     * @param motionTarget The target position to determine the safety of
+     * @return Is the motion safe
+     */
+    public boolean isMotionSafe(double motionTarget) {
+        return motionTarget > ArmConstants.kArmSafeStowPosition;
+    }
+
     //manual command that keeps ouput speed consistent no matter the direction
     public Command runArm(DoubleSupplier speed) {
         return run(() -> {

src/main/java/frc/robot/subsystems/Elevator.java

@@ -81,6 +81,27 @@ public class Elevator extends SubsystemBase {
         );
     }
 
+    /**
+     * Returns whether or not the motion is safe relative to the encoder's current position
+     * and the elevator brace position
+     * 
+     * @return Is the motion safe
+     */
+    public boolean isMotionSafe() {
+        return isMotionSafe(getEncoderPosition());
+    }
+
+    /**
+     * Returns whether or not the motion is safe relative to some target position and the elevator
+     * brace position
+     * 
+     * @param motionTarget The target position to determine the safety of
+     * @return Is the motion safe
+     */
+    public boolean isMotionSafe(double motionTarget) {
+        return motionTarget > ElevatorConstants.kElevatorBracePosition;
+    }
+
     //manual command that keeps ouput speed consistent no matter the direction
     public Command runElevator(DoubleSupplier speed) {
         return run(() -> {