Elevator moves to intake much faster

src/main/java/frc/robot/CommandSequences.java

@@ -39,12 +39,16 @@ public CommandSequences(Drive drive, Led led, AlgaeDestage algaeDestage, CoralMa
     // Intakes until sensor is tripped, LEDs indicate that game piece is acquired
 
     public Command intake() {
-        return armRotate.moveTo(ArmRotatePresets.OUT).andThen(led.elevatorToPreset(),
+        return armRotate.moveIntaking().andThen(led.elevatorToPreset(),
                 elevator.moveTo(ElevatorPresets.INTAKE), led.elevatorAtPreset(),
                 armRotate.moveTo(ArmRotatePresets.INTAKE),
                 led.intaking(), coralManip.intake(), led.gamePieceAcquired());
     }
 
+    public Command intakeBottom() {
+        return armRotate.moveTo(ArmRotatePresets.INTAKE).alongWith(coralManip.intake());
+    }
+
     // Moves elevator to set coral preset
 
     public Command moveElevator(ElevatorPresets level, ArmRotatePresets preset) {

src/main/java/frc/robot/Robot.java

@@ -22,6 +22,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.maps.RobotMap;
 import frc.robot.maps.subsystems.ArmRotateMap.ArmRotatePresets;
 import frc.robot.maps.subsystems.ElevatorMap.ElevatorPresets;
@@ -39,6 +40,7 @@ public final class Robot extends CommandRobot {
     private RobotMap map = getRobotMap(RobotMap.class, new RobotMap());
     private ButtonXboxController driveController = new ButtonXboxController(0);
     private ButtonXboxController copilotController = new ButtonXboxController(1);
+    private Trigger elevatorSafeTrigger;
 
     // Helpers
     final DoubleUnaryOperator driveScaler = getScaler(0.45, 0.25);
@@ -88,6 +90,7 @@ public Robot() {
         super();
         registerNamedCommands();
         autoChooser = AutoBuilder.buildAutoChooser();
+        elevatorSafeTrigger = new Trigger(() -> elevator.elevatorSafeTrigger().getAsBoolean());
     }
 
     @Override
@@ -140,6 +143,7 @@ public void configureButtonBindings() {
                 .whileTrue(drive.robotCentricDrive());
 
         copilotController.a().onTrue(commandSequences.intake());
+        elevatorSafeTrigger.onTrue(commandSequences.intakeBottom());
 
         copilotController.x()
                 .whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL1, ArmRotatePresets.SCOREL1))

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

@@ -66,6 +66,15 @@ public Command moveTo(ArmRotatePresets level) {
         }).until(setPointPersistenceCheck)).withName("Move To Set Angle");
     }
 
+    public Command moveIntaking() {
+        return runOnce(() -> {
+            this.preset = ArmRotatePresets.OUT;
+            pid.reset(getArmAngle(), 0.0);
+        }).andThen(run(() -> {
+            Logger.recordOutput("Arm pid at goal", pid.atGoal());
+        }).until(() -> (getData().rotationAbsAngleDegrees < 67))).withName("Move To Set Angle");
+    }
+
     public Command zero() {
         return runSafe(() -> {
             getData().motor.setpoint = ZEROING_SPEED;

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

@@ -10,7 +10,7 @@
 
 public class CoralManip extends LoggedSubsystem<Data, CoralManipMap> {
 
-    private final double RELEASE_SPEEDRIGHT = -0.5;
+    private final double RELEASE_SPEEDRIGHT = -0.3;
     private final double RELEASE_SPEEDLEFT = -0.1;
     private final double INTAKE_SPEED = -0.3;
     private final double RELEASE_DELAY = 1;

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

@@ -1,5 +1,6 @@
 package frc.robot.subsystems;
 
+import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
 import org.littletonrobotics.junction.Logger;
@@ -64,25 +65,32 @@ public Command zero() {
         return startSafe(() -> {
             getMap().motor.resetValidators();
             level = ElevatorPresets.OFF;
-                getData().motor.setpoint = ZEROING_SPEED;
-            
+            getData().motor.setpoint = ZEROING_SPEED;
+
         }).until(() -> getMap().motor.validate() && armSafeSub.getAsBoolean()).andThen(resetCmd());
     }
 
     public Command moveTo(ElevatorPresets level) {
         PersistenceCheck setPointPersistenceCheck = new PersistenceCheck(30, pid::atGoal);
         return runOnce(() -> {
-                this.level = level;
-                pid.reset(getElevatorHeight(), getData().liftingHeightVelocity);
-            
+            this.level = level;
+            pid.reset(getElevatorHeight(), getData().liftingHeightVelocity);
+
         }).andThen(run(() -> {
             Logger.recordOutput("PID at goal", pid.atGoal());
+            hold();
         })).until(() -> {
             return setPointPersistenceCheck.getAsBoolean() && armSafeSub.getAsBoolean();
         }).withName("Move to set height");
 
     }
 
+    public BooleanSupplier elevatorSafeTrigger() {
+        return () -> {
+            return (getData().heightAbsInches < 12 && level == ElevatorPresets.INTAKE);
+        };
+    }
+
     public Command hold() {
         return runOnce(() -> {
             holdHeight = getElevatorHeight();