From ce7246114f48d6f815d31777798b0f24f7d034b2 Mon Sep 17 00:00:00 2001
From: NoahLacks63 <apexdeadlyranger@gmail.com>
Date: Tue, 21 Jan 2025 00:58:38 +0000
Subject: [PATCH] Added lots of comments, also added a few simple methods as
 backup

---
 src/main/java/frc/robot/RobotContainer.java   | 14 +++++-
 .../robot/constants/ElevatorConstants.java    |  1 +
 .../java/frc/robot/subsystems/Elevator.java   | 47 +++++++++++++++++--
 3 files changed, 55 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 122243f..3343d56 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -86,7 +86,7 @@ public class RobotContainer {
 		);
 
 		elevator.setDefaultCommand(
-			elevator.runElevator(operator::getLeftY)
+			elevator.runAssistedElevator(operator::getLeftY)
 		);
 
 		indexer.setDefaultCommand(
@@ -222,4 +222,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));
+	}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java
index 21fab1b..2367a0b 100644
--- a/src/main/java/frc/robot/constants/ElevatorConstants.java
+++ b/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;
diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java
index 7c7fb7d..4c7f2b3 100644
--- a/src/main/java/frc/robot/subsystems/Elevator.java
+++ b/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();
+    }
+}
\ No newline at end of file