1,000 comments, reworked the climber pivot, removed indexer, added clamps on goToSetpoint methods

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

@@ -12,12 +12,12 @@ import frc.robot.subsystems.ClimberPivot;
 import frc.robot.subsystems.ClimberRollers;
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.Elevator;
-import frc.robot.subsystems.Indexer;
 import frc.robot.subsystems.Manipulator;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -37,8 +37,6 @@ public class RobotContainer {
 
 	private Elevator elevator;
 
-	private Indexer indexer;
-
 	private Manipulator manipulator;
 
 	private CommandXboxController driver;
@@ -57,8 +55,6 @@ public class RobotContainer {
 
 		elevator = new Elevator();
 
-		indexer = new Indexer();
-
 		manipulator = new Manipulator();
 
 		driver = new CommandXboxController(OIConstants.kDriverControllerPort);
@@ -74,12 +70,13 @@ public class RobotContainer {
 	}
 
 	private void configureButtonBindings() {
+		//Default commands
 		arm.setDefaultCommand(
 			arm.goToSetpoint(0, 1)
 		);
 
 		climberPivot.setDefaultCommand(
-			climberPivot.goToAngle(0, 1)
+			climberPivot.runPivot(0)
 		);
 
 		climberRollers.setDefaultCommand(
@@ -99,12 +96,8 @@ public class RobotContainer {
 			elevator.runAssistedElevator(operator::getLeftY)
 		);
 
-		indexer.setDefaultCommand(
-			indexer.runIndexer(0)
-		);
-
 		manipulator.setDefaultCommand(
-			manipulator.runManipulator(0)
+			manipulator.runManipulator(() -> 0, true)
 		);
 
 		//Driver inputs
@@ -113,7 +106,7 @@ public class RobotContainer {
 		);
 
 		driver.rightTrigger().whileTrue(
-			manipulator.runManipulator(1)
+			manipulator.runManipulator(() -> 1, true)
 		);
 
 		//Operator inputs
@@ -189,7 +182,10 @@ public class RobotContainer {
 		return autoChooser.getSelected();
 	}
 
-	//teleop routines
+	/**
+	 * Moves the elevator and arm to the coral intake position, then runs the manipulator until collected
+	 * @return Moves the elevator and arm, then intakes coral
+	 */
 	private Command coralIntakeRoutine() {
 		return moveManipulator(
 			ElevatorConstants.kElevatorCoralIntakePosition, 
@@ -198,6 +194,12 @@ public class RobotContainer {
 			.andThen(manipulator.runUntilCollected(1, true));
 	}
 
+	/**
+	 * Moves the elevator and arm to the constant setpoints and runs the manipulator until collected
+	 * 
+	 * @param l2 Is the algae on L2? (True = L2, False = L3)
+	 * @return Moves the elevator and arm then intakes algae
+	 */
 	private Command algaeIntakeRoutine(boolean l2) {
 		return moveManipulator(
 			l2 ? ElevatorConstants.kElevatorL2AlgaePosition : ElevatorConstants.kElevatorL3AlgaePosition, 
@@ -206,6 +208,13 @@ public class RobotContainer {
 			.andThen(manipulator.runUntilCollected(1, false));
 	}
 
+	/**
+	 * Moves the elevator and arm in different order based on target positions
+	 * 
+	 * @param elevatorPosition The target position of the elevator
+	 * @param armPosition The target rotation of the arm
+	 * @return Moves the elevator and arm to the setpoints using the most efficient path
+	 */
 	private Command moveManipulator(double elevatorPosition, double armPosition) {
 		// If the elevator current and target positions are above the brace, or the arm current and target position is in
 		// front of the brace, move together
@@ -229,7 +238,24 @@ public class RobotContainer {
 		}
 	}
 
+	/**
+	 * Moves the elevator and arm in customizeable ways
+	 * 
+	 * @param elevatorPosition The target elevator position
+	 * @param armPosition The target arm position
+	 * @param elevatorFirst Does the elevator move first? (True = Elevator first, False = Arm first)
+	 * @param sequential Does the elevator and arm move separately? (True = .andThen, False = .alongWith)
+	 * @return Moves the elevator and arm to the setpoints
+	 */
 	private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
+		if (elevatorPosition <= ElevatorConstants.kElevatorBracePosition || elevatorPosition == 0) {
+			armPosition = MathUtil.clamp(
+				armPosition, 
+				0,
+				ArmConstants.kArmRotationLimit
+			);
+		}
+		
 		return Commands.either(
 			Commands.either(
 				elevator.goToSetpoint(elevatorPosition, 2).andThen(arm.goToSetpoint(armPosition, 2)),
@@ -245,9 +271,12 @@ public class RobotContainer {
 		);
 	}
 
-	/*
+	/**
-	 * A moveManipulator method that will guarantee a safe movement. 
+	 * Moves the arm and elevator in a safe way.
-	 * Here in case we need want to skip moveManipulator debugging
+	 * 
+	 * @param elevatorPosition The target position of the elevator
+	 * @param armPosition The target rotation of the arm
+	 * @return Moves the elevator and arm to the setpoints
 	 */
 	@SuppressWarnings("unused")
 	private Command safeMoveManipulator(double elevatorPosition, double armPosition) {

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

@@ -39,7 +39,10 @@ public class ArmConstants {
     public static final double kArmL4Position = 0;
     public static final double kArmL2AlgaePosition = 0;
     public static final double kArmL3AlgaePosition = 0;
+    /**The closest position to the elevator brace without hitting it */
     public static final double kArmSafeStowPosition = 0;
+    /**The forward rotation limit of the arm */
+    public static final double kArmRotationLimit = 0;
 
     public static final double kMagnetOffset = 0.0;
     public static final double kAbsoluteSensorDiscontinuityPoint = 0.0;

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

@@ -10,4 +10,6 @@ public class ClimberPivotConstants {
     public static final double kPIDControllerP = 0;
     public static final double kPIDControllerI = 0;
     public static final double kPIDControllerD = 0;
+
+    public static final double kClimberClimbPosition = 0;
 }

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

@@ -42,6 +42,7 @@ public class ElevatorConstants {
     public static final double kElevatorL4Position = 0;
     public static final double kElevatorL2AlgaePosition = 0;
     public static final double kElevatorL3AlgaePosition = 0;
+    /**The position of the top of the elevator brace */
     public static final double kElevatorBracePosition = 0;
     public static final double kElevatorMaxHeight = 0;
 

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

@@ -1,6 +0,0 @@
-package frc.robot.constants;
-
-public class IndexerConstants {
-    public static final int kIndexerMotorID = 0;
-    public static final int kIndexerBeamBreakID = 0;
-}

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

@@ -8,6 +8,7 @@ import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.util.Units;
@@ -76,10 +77,21 @@ public class Arm extends SubsystemBase {
         return motionTarget > ArmConstants.kArmSafeStowPosition;
     }
 
-    //manual command that keeps ouput speed consistent no matter the direction
+    /**
-    public Command runArm(DoubleSupplier speed) {
+     * A manual rotation command that will move the elevator using a consistent velocity disregarding direction
+     * 
+     * @param speed The velocity at which the arm rotates
+     * @return Sets motor voltage to achieve the target velocity
+     */
+    public Command runAssistedArm(DoubleSupplier speed) {
+        double clampedSpeed = MathUtil.clamp(
+            speed.getAsDouble(), 
+            -1, 
+            1
+        );
+
         return run(() -> {
-            double realSpeedTarget = speed.getAsDouble() * ArmConstants.kArmMaxVelocity;
+            double realSpeedTarget = clampedSpeed * ArmConstants.kArmMaxVelocity;
 
             double voltsOut = velocityController.calculate(
                 getEncoderVelocity(), 
@@ -93,11 +105,24 @@ public class Arm extends SubsystemBase {
         });
     }
 
+    /**
+     * Moves the arm 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) {
+        double clampedSetpoint = MathUtil.clamp(
+            setpoint, 
+            0, 
+            ArmConstants.kArmRotationLimit
+        );
+
         return run(() -> {
             double voltsOut = positionController.calculate(
                 getEncoderPosition(), 
-                setpoint
+                clampedSetpoint
             ) + feedForward.calculate(
                 getEncoderPosition(), 
                 getEncoderVelocity()
@@ -107,10 +132,20 @@ public class Arm extends SubsystemBase {
         }).until(positionController::atSetpoint).withTimeout(timeout);
     }
 
+    /**
+     * Returns the CANCoder's position in radians
+     * 
+     * @return CANCoder's position in radians
+     */
     public double getEncoderPosition() {
         return Units.rotationsToRadians(canCoder.getAbsolutePosition().getValueAsDouble());
     }
 
+    /**
+     * Returns the CANCoder's velocity in radians per second
+     * 
+     * @return CANCoder's velocity in radians per second
+     */
     public double getEncoderVelocity() {
         return Units.rotationsToRadians(canCoder.getVelocity().getValueAsDouble());
     }

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

@@ -4,7 +4,6 @@ import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
-import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -17,8 +16,6 @@ public class ClimberPivot extends SubsystemBase {
 
     private DigitalInput cageLimitSwitch;
 
-    private PIDController pidController;
-
     public ClimberPivot() {
         pivotMotor = new SparkMax(
             ClimberPivotConstants.kPivotMotorID, 
@@ -28,12 +25,6 @@ public class ClimberPivot extends SubsystemBase {
         neoEncoder = pivotMotor.getEncoder();
 
         cageLimitSwitch = new DigitalInput(ClimberPivotConstants.kClimberLimitSwitchID);
-
-        pidController = new PIDController(
-            ClimberPivotConstants.kPIDControllerP,
-            ClimberPivotConstants.kPIDControllerI, 
-            ClimberPivotConstants.kPIDControllerD
-        );
     }
 
     public Command runPivot(double speed) {
@@ -42,17 +33,23 @@ public class ClimberPivot extends SubsystemBase {
         });
     }
 
-    public Command goToAngle(double setpoint, double timeout) {
+    /**
+     * Runs the climber until it is at a defined setpoint
+     * 
+     * @param speed The speed at which the pivot runs
+     * @return Sets the motor speed until at the climbed position
+     */
+    public Command climb(double speed) {
         return run(() -> {
-            pivotMotor.set(
+            pivotMotor.set(speed);
-                pidController.calculate(
+        }).until(() -> neoEncoder.getPosition() >= ClimberPivotConstants.kClimberClimbPosition);
-                    neoEncoder.getPosition(), 
-                    setpoint
-                )
-            );
-        }).withTimeout(timeout);
     }
 
+    /**
+     * Returns the limit switch attached to the climber. Detects if the cage is present
+     * 
+     * @return Is the cage in the climber
+     */
     public boolean getCageLimitSwitch() {
         return cageLimitSwitch.get();
     }

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

@@ -17,6 +17,12 @@ public class ClimberRollers extends SubsystemBase {
         );
     }
 
+    /**
+     * Runs the rollers at a set speed
+     * 
+     * @param speed The speed in which the roller runs
+     * @return Runs the rollers at a set speed
+     */
     public Command runRoller(double speed) {
         return run(() -> {
             rollerMotor.set(speed);

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

@@ -8,6 +8,7 @@ import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.DigitalInput;
@@ -76,6 +77,13 @@ public class Elevator extends SubsystemBase {
         );
     }
 
+    @Override
+    public void periodic() {
+        if (getBottomLimitSwitch()) {
+            encoder.setPosition(0);
+        }
+    }
+
     /**
      * Returns whether or not the motion is safe relative to the encoder's current position
      * and the elevator brace position
@@ -96,6 +104,18 @@ public class Elevator extends SubsystemBase {
     public boolean isMotionSafe(double motionTarget) {
         return motionTarget > ElevatorConstants.kElevatorBracePosition;
     }
+    
+    /**
+     * 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);
+        });
+    }
 
     /**
      * A manual translation command that will move the elevator using a consistent velocity disregarding direction
@@ -116,18 +136,6 @@ public class Elevator extends SubsystemBase {
         }).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);
-        });
-    }
     
     /**
      * Moves the elevator to a target destination (setpoint)
@@ -137,10 +145,16 @@ public class Elevator extends SubsystemBase {
      * @return Sets motor voltage to achieve the target destination
      */
     public Command goToSetpoint(double setpoint, double timeout) {
+        double clampedSetpoint = MathUtil.clamp(
+            setpoint, 
+            0, 
+            ElevatorConstants.kElevatorMaxHeight
+        );
+
         return run(() -> {
             double voltsOut = positionController.calculate(
                 encoder.getPosition(),
-                setpoint
+                clampedSetpoint
             ) + feedForward.calculate(0);
 
             elevatorMotor1.setVoltage(voltsOut);

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

@@ -1,36 +0,0 @@
-package frc.robot.subsystems;
-
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.IndexerConstants;
-
-public class Indexer extends SubsystemBase {
-    private SparkMax indexerMotor;
-
-    private DigitalInput indexerBeamBreak;
-
-    public Indexer() {
-        indexerMotor = new SparkMax(
-            IndexerConstants.kIndexerMotorID, 
-            MotorType.kBrushless
-        );
-
-        indexerBeamBreak = new DigitalInput(IndexerConstants.kIndexerBeamBreakID);
-    }
-
-    public Command runIndexer(double speed) {
-        return run(() -> {
-            indexerMotor.set(speed);
-        });
-    }
-
-    public Command indexCoral(double speed) {
-        return run(() -> {
-            indexerMotor.set(speed);
-        }).until(indexerBeamBreak::get);
-    }
-}

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

@@ -1,5 +1,7 @@
 package frc.robot.subsystems;
 
+import java.util.function.DoubleSupplier;
+
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
@@ -24,12 +26,26 @@ public class Manipulator extends SubsystemBase {
         algaeBeamBreak = new DigitalInput(ManipulatorConstants.kAlgaeBeamBreakID);
     }
 
-    public Command runManipulator(double speed) {
+    /**
+     * Runs the manipulator at a set speed with the direction based on the coral parameter
+     * 
+     * @param speed The speed at which the manipulator runs
+     * @param coral Is the manipulator manipulating a coral? (True = Coral, False = Algae)
+     * @return Runs manipulator at speed
+     */
+    public Command runManipulator(DoubleSupplier speed, boolean coral) {
         return run(() -> {
-            manipulatorMotor.set(speed);
+            manipulatorMotor.set(coral ? speed.getAsDouble() : speed.getAsDouble() * -1);
         });
     }
 
+    /**
+     * Runs the manipulator until either the algae or coral beam break reads true
+     * 
+     * @param speed The speed at which the manipulator is run
+     * @param coral Is the object a coral? (True = Coral, False = Algae)
+     * @return Runs the manipulator until an object is present
+     */
     public Command runUntilCollected(double speed, boolean coral) {
         return run(() -> {
             manipulatorMotor.set(coral ? speed : speed * -1);