added a smooth manipulator translation method

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

@@ -14,6 +14,9 @@ import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.Indexer;
 import frc.robot.subsystems.Manipulator;
+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.PrintCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -55,6 +58,8 @@ public class RobotContainer {
 		operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
 
 		configureButtonBindings();
+
+		configureShuffleboard();
 	}
 
 	private void configureButtonBindings() {
@@ -132,8 +137,31 @@ public class RobotContainer {
 		operator.a().onTrue(
 			coralIntakeRoutine()
 		);
+
+		operator.x().onTrue(
+			algaeIntakeRoutine(true)
+		);
+
+		operator.b().onTrue(
+			algaeIntakeRoutine(false)
+		);
 	}
 
+	  //creates tabs and transforms them on the shuffleboard//
+  private void configureShuffleboard() {
+    ShuffleboardTab sensorTab = Shuffleboard.getTab(OIConstants.kSensorsTab);
+
+    sensorTab.addDouble("ElevatorPosition", elevator::getEncoderPosition)
+		.withSize(2, 1)
+		.withPosition(0, 0)
+		.withWidget(BuiltInWidgets.kTextView);
+
+	sensorTab.addDouble("ArmPosition", arm::getEncoderPosition)
+		.withSize(2, 1)
+		.withPosition(2, 0)
+		.withWidget(BuiltInWidgets.kTextView);
+  }
+
 	public Command getAutonomousCommand() {
 		return new PrintCommand("NO AUTO DEFINED");
 	}
@@ -144,16 +172,37 @@ public class RobotContainer {
 			ElevatorConstants.kElevatorCoralIntakePosition, 
 			ArmConstants.kArmCoralIntakePosition
 		)
-			.andThen(manipulator.indexCoral(.5));
+			.andThen(manipulator.runUntilCollected(1, true));
+	}
+
+	private Command algaeIntakeRoutine(boolean l2) {
+		return moveManipulator(
+			l2 ? ElevatorConstants.kElevatorL2AlgaePosition : ElevatorConstants.kElevatorL3AlgaePosition, 
+			l2 ? ArmConstants.kArmL2AlgaePosition : ArmConstants.kArmL3AlgaePosition
+		)
+			.andThen(manipulator.runUntilCollected(1, false));
 	}
 
 	private Command moveManipulator(double elevatorPosition, double armPosition) {
-		if ((arm.getEncoderPosition() < ArmConstants.kArmSafeStowPosition && armPosition < ArmConstants.kArmSafeStowPosition) || (elevator.getEncoderPosition() > ElevatorConstants.kElevatorBracePosition && elevatorPosition > ElevatorConstants.kElevatorBracePosition)) {
+		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
 			return arm.goToSetpoint(armPosition, 2)
 				.alongWith(elevator.goToSetpoint(elevatorPosition, 2));
-		} else {
+		} else if (armPosition < ArmConstants.kArmSafeStowPosition) { //if the target is behind the brace, move the elevator first
+			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
+				return arm.goToSetpoint(ArmConstants.kArmSafeStowPosition, 2)
+					.andThen(elevator.goToSetpoint(elevatorPosition, 2))
+						.andThen(arm.goToSetpoint(armPosition, 2));
+			} else { //if the arm is in front of the brace, move elevator, then arm
+				return elevator.goToSetpoint(elevatorPosition, 2)
+					.andThen(arm.goToSetpoint(armPosition, 2));
+			}
+		} else if (arm.getEncoderPosition() < ArmConstants.kArmSafeStowPosition) { //if the arm is behind the brace, move to target, then move the elevator
 			return arm.goToSetpoint(armPosition, 2)
 				.andThen(elevator.goToSetpoint(elevatorPosition, 2));
+		} else { //catch all that will always safely move the arm in a safe way
+			return arm.goToSetpoint(ArmConstants.kArmSafeStowPosition, 2)
+				.andThen(elevator.goToSetpoint(elevatorPosition, 2))
+					.andThen(arm.goToSetpoint(armPosition, 2));
 		}
 	}
 }

src/main/java/frc/robot/constants/OIConstants.java

@@ -5,4 +5,6 @@ public class OIConstants {
     public static final int kOperatorControllerPort = 1;
     
     public static final double kDriveDeadband = 0.05;
+
+    public static final String kSensorsTab = "SensorsTab";
 }

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

@@ -35,10 +35,4 @@ public class Manipulator extends SubsystemBase {
             manipulatorMotor.set(coral ? speed : speed * -1);
         }).until(() -> coralBeamBreak.get() || algaeBeamBreak.get());
     }
-
-    public Command indexCoral(double speed) {
-        return run(() -> {
-            manipulatorMotor.set(speed);
-        }).until(coralBeamBreak::get);
-    }
 }