split up align for handoff and handoff coral

Author: rift10

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

@@ -85,7 +85,6 @@ public class RobotContainer {
     @Getter
     private final SendableChooser<Command> autoChooser;
 
-
     public RobotContainer() {
         // Get driver station to stop
         DriverStation.silenceJoystickConnectionWarning(true);

src/main/java/frc/robot/commands/intake/AlignForHandoff.java

@@ -0,0 +1,38 @@
+package frc.robot.commands.intake;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.intake.pivot.IntakePivot;
+import frc.robot.subsystems.intake.pivot.IntakePivot.IntakePivotGoal;
+import frc.robot.subsystems.outtake.wrist.Wrist;
+import frc.robot.subsystems.outtake.wrist.Wrist.WristGoal;
+
+public class AlignForHandoff extends Command {
+
+    private final Wrist wrist;
+    private final IntakePivot pivot;
+
+    public AlignForHandoff(RobotContainer robot) {
+        pivot = robot.getIntakePivot();
+        wrist = robot.getWrist();
+
+        addRequirements(pivot, wrist);
+    }
+
+    @Override
+    public void initialize() {
+        pivot.runPosition(IntakePivotGoal.TO_INTAKE_HANDOFF);
+        wrist.runPosition(WristGoal.HANDOFF);
+    }
+
+    @Override
+    public void execute() {}
+
+    @Override
+    public boolean isFinished() {
+        return pivot.atGoal() && wrist.atGoal();
+    }
+
+    @Override
+    public void end(boolean isFinished) {}
+}

src/main/java/frc/robot/commands/intake/HandoffCoral.java

@@ -4,57 +4,48 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.RobotContainer;
 import frc.robot.subsystems.beambreak.Beambreak;
-import frc.robot.subsystems.intake.pivot.IntakePivot;
-import frc.robot.subsystems.intake.pivot.IntakePivot.IntakePivotGoal;
 import frc.robot.subsystems.intake.roller.IntakeRoller;
 import frc.robot.subsystems.intake.roller.IntakeRoller.IntakeRollerGoal;
 import frc.robot.subsystems.outtake.endeffector.EndEffector;
 import frc.robot.subsystems.outtake.endeffector.EndEffector.EndEffectorRollerGoal;
-import frc.robot.subsystems.outtake.wrist.Wrist;
-import frc.robot.subsystems.outtake.wrist.Wrist.WristGoal;
 
 public class HandoffCoral extends Command {
 
     private static final double kTimeOutTime = 10.0;
     private final IntakeRoller roller;
     private final EndEffector endEffector;
-    private final Wrist wrist;
-    private final IntakePivot pivot;
     private final Beambreak beamBreak;
     private final Timer timer;
 
     public HandoffCoral(RobotContainer robot) {
         roller = robot.getIntakeRoller(); // fix later
         endEffector = robot.getEndEffector();
         beamBreak = robot.getBeamBreak();
-        pivot = robot.getIntakePivot();
-        wrist = robot.getWrist();
         timer = new Timer();
 
-        addRequirements(roller, endEffector, pivot, wrist);
+        addRequirements(roller, endEffector);
     }
 
+    @Override
     public void initialize() {
-        pivot.runPosition(IntakePivotGoal.TO_INTAKE_HANDOFF);
-        wrist.runPosition(WristGoal.HANDOFF);
         timer.restart();
     }
 
+    @Override
     public void execute() {
-        if (pivot.atGoal() && wrist.atGoal()) {
-            roller.setGoal(IntakeRollerGoal.OUTTAKEENDEFFECTOR);
-            endEffector.setGoal(EndEffectorRollerGoal.INTAKE);
-        }
+        roller.setGoal(IntakeRollerGoal.OUTTAKEENDEFFECTOR);
+        endEffector.setGoal(EndEffectorRollerGoal.INTAKE);
     }
 
+    @Override
     public boolean isFinished() {
         return beamBreak.gamepieceInEndEffector()
                 || timer.hasElapsed(kTimeOutTime)
                 || !beamBreak.coralInIntake(); // in case of mechanical failure
     }
 
+    @Override
     public void end(boolean isFinished) {
         roller.setGoal(IntakeRollerGoal.IDLE);
-        // endEffector.setGoal(EndEffectorRollerGoal.IDLE);
     }
 }

src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java

@@ -13,7 +13,7 @@ public class ElevatorConstants {
 
     public static final double kGearRatio =
             switch (GlobalConstants.getRobotType()) {
-                    case BETA -> 5.0;
+                case BETA -> 5.0;
                 default -> 5.0;
             };