fast automatic climb lowering (for auton)

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

@@ -39,6 +39,7 @@
 import edu.wpi.first.wpilibj2.command.button.POVButton;
 import frc.robot.Constants.SwerveConstants;
 import frc.robot.commands.auton.AutonBuilder;
+import frc.robot.commands.climb.ClimbLowerCommand;
 import frc.robot.commands.elevator.ElevatorToAmpCommand;
 import frc.robot.commands.elevator.ElevatorToEncoderZeroCommand;
 import frc.robot.commands.elevator.ElevatorToTrapCommand;
@@ -111,6 +112,7 @@ public class RobotContainer {
     private final JoystickButton rightStickButton = new JoystickButton(mechController,
         XboxController.Button.kRightStick.value);
     private final POVButton dPadRight = new POVButton(mechController, 90);
+    private final JoystickButton startButton = new JoystickButton(mechController, XboxController.Button.kStart.value);
 
     private final GenericHID switchboard = new GenericHID(3);
     private final JoystickButton offsetUpButton = new JoystickButton(switchboard, 7);
@@ -227,6 +229,7 @@ public RobotContainer() {
     private void configureBindings() {
         /* Test Bindings -- Leave these commented out when not needed. */
         // leftStickButton.onTrue(new CalculateBackCameraTransformCommand(BACK_LEFT_CAMERA, BACK_RIGHT_CAMERA));
+        startButton.onTrue(new ClimbLowerCommand(climbSubsystem));
 
         /* Driving -- One joystick controls translation, the other rotation. If the robot-relative button is held down,
          * the robot is controlled along its own axes, otherwise controls apply to the field axes by default. If the

src/main/java/frc/robot/commands/climb/ClimbLowerCommand.java

@@ -0,0 +1,33 @@
+package frc.robot.commands.climb;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.climb.ClimbSubsystem;
+
+/** Lowers climb fully when off the chain. Should run at the start of auton to ensure that both arms are at their lowest
+ *  positions so that the robot may fit underneath the stage. */
+public class ClimbLowerCommand extends Command {
+    private static final double LOWERING_SPEED = -0.9;
+    private final ClimbSubsystem climbSubsystem;
+
+    /** Constructs a new {@link ClimbLowerCommand}. */
+    public ClimbLowerCommand(ClimbSubsystem climbSubsystem) {
+        this.climbSubsystem = climbSubsystem;
+
+        this.addRequirements(climbSubsystem);
+    }
+
+    @Override
+    public void initialize() {
+        climbSubsystem.setSpeeds(LOWERING_SPEED, LOWERING_SPEED);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return climbSubsystem.isLowered();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        super.end(interrupted);
+    }
+}

src/main/java/frc/robot/subsystems/climb/ClimbArm.java

@@ -56,7 +56,7 @@ public void update(double speed) {
 
         if (this.isLimitSwitchPressed()) {
             resetEncoder();
-            winchPower = Math.max(0, winchPower);
+            winchPower = Math.max(-.1, winchPower);
         }
 
         if (winchPower != prevWinchPower) {

src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java

@@ -60,4 +60,9 @@ public void enableAutomaticLowering(boolean enable) {
     public void toggleAutomaticLowering() {
         keepLowered = !keepLowered;
     }
+
+    /** Returns true if both climb arms are at their lowest positions. */
+    public boolean isLowered() {
+        return rightClimbArm.isLimitSwitchPressed() && leftClimbArm.isLimitSwitchPressed();
+    }
 }