Merge branch 'main' of https://git.coldlightalchemist.com/Team_2648/2025_Robot_Code

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

@@ -96,7 +96,7 @@ public class RobotContainer {
 		);
 
 		elevator.setDefaultCommand(
-			elevator.runElevator(operator::getLeftY)
+			elevator.runAssistedElevator(operator::getLeftY)
 		);
 
 		indexer.setDefaultCommand(
@@ -244,4 +244,14 @@ public class RobotContainer {
 			() -> elevatorFirst
 		);
 	}
-}
+
+	/*
+	 * A moveManipulator method that will guarantee a safe movement. 
+	 * Here in case we need want to skip moveManipulator debugging
+	 */
+	@SuppressWarnings("unused")
+	private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
+		return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
+			.andThen(arm.goToSetpoint(armPosition, 2));
+	}
+}

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

@@ -43,6 +43,7 @@ public class ElevatorConstants {
     public static final double kElevatorL2AlgaePosition = 0;
     public static final double kElevatorL3AlgaePosition = 0;
     public static final double kElevatorBracePosition = 0;
+    public static final double kElevatorMaxHeight = 0;
 
     // 1, 7, 10 are the defaults for these, change as necessary
     public static final double kSysIDRampRate = 1;

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

@@ -97,8 +97,13 @@ public class Elevator extends SubsystemBase {
         return motionTarget > ElevatorConstants.kElevatorBracePosition;
     }
 
-    //manual command that keeps ouput speed consistent no matter the direction
-    public Command runElevator(DoubleSupplier speed) {
+    /**
+     * A manual translation command that will move the elevator using a consistent velocity disregarding direction
+     * 
+     * @param speed How fast the elevator moves
+     * @return Sets motor voltage to move the elevator relative to the speed parameter
+     */
+    public Command runAssistedElevator(DoubleSupplier speed) {
         return run(() -> {
             double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kElevatorMaxVelocity;
 
@@ -108,11 +113,29 @@ public class Elevator extends SubsystemBase {
             ) + feedForward.calculate(realSpeedTarget);
 
             elevatorMotor1.setVoltage(voltsOut);
-        }).until(bottomLimitSwitch::get);
+        }).until(
+            () -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kElevatorMaxHeight);
     }
 
+    /**
+     * A manual translation command that uses feed forward calculation to maintain position
+     * 
+     * @param speed The speed at which the elevator translates
+     * @return Sets motor voltage to translate the elevator and maintain position
+     */
+    public Command runManualElevator(double speed) {
+        return run(() -> {
+            elevatorMotor1.set(speed);
+        });
+    }
     
-    //go to setpoint command
+    /**
+     * Moves the elevator to a target destination (setpoint)
+     * 
+     * @param setpoint Target destination of the subsystem
+     * @param timeout Time to achieve the setpoint before quitting
+     * @return Sets motor voltage to achieve the target destination
+     */
     public Command goToSetpoint(double setpoint, double timeout) {
         return run(() -> {
             double voltsOut = positionController.calculate(
@@ -126,7 +149,21 @@ public class Elevator extends SubsystemBase {
         ).withTimeout(timeout);
     }
 
+    /**
+     * Returns the current encoder position
+     * 
+     * @return Current encoder position
+     */
     public double getEncoderPosition() {
         return encoder.getPosition();
     }
-}
+
+    /**
+     * Returns the value of the bottom limit switch on the elevator (false = disabled, true = enabled)
+     * 
+     * @return The value of bottomLimitSwitch
+     */
+    public boolean getBottomLimitSwitch() {
+        return bottomLimitSwitch.get();
+    }
+}