diff --git a/src/main/java/frc/robot/CommandFactory.java b/src/main/java/frc/robot/CommandFactory.java index afb928c8..d4281444 100644 --- a/src/main/java/frc/robot/CommandFactory.java +++ b/src/main/java/frc/robot/CommandFactory.java @@ -163,11 +163,12 @@ private Command formSafeMovementCommand(Position pos) { } else { if (currentHeight < ELEVATOR_PIVOT_LIMBO || goalHeight < currentHeight || goalAngle < currentAngle) { commands.addCommands(new PivotMoveCommand(subsystemManager.getPivotSubsystem(), Position.SAFE_EXTEND)); - commands.addCommands(new ElevatorMoveCommand(subsystemManager.getElevatorSubsystem(), pos)); - commands.addCommands(new PivotMoveCommand(subsystemManager.getPivotSubsystem(), pos)); - } else { - commands.addCommands(new ElevatorMoveCommand(subsystemManager.getElevatorSubsystem(), pos)); - commands.addCommands(new PivotMoveCommand(subsystemManager.getPivotSubsystem(), pos)); + } + + commands.addCommands(new ElevatorMoveCommand(subsystemManager.getElevatorSubsystem(), pos)); + + if (!RobotIO.getInstance().getInternalCoralDetectorOutput().hasCoral()){ + commands.addCommands(new PivotMoveCommand(subsystemManager.getPivotSubsystem(), pos)); } } return commands; diff --git a/src/main/java/frc/robot/operation/OperatorInterface.java b/src/main/java/frc/robot/operation/OperatorInterface.java index d416454a..34f239bc 100644 --- a/src/main/java/frc/robot/operation/OperatorInterface.java +++ b/src/main/java/frc/robot/operation/OperatorInterface.java @@ -221,6 +221,7 @@ public void operatorBindings() { () -> RobotIO.getInstance().getInternalCoralDetectorOutput().hasCoral() ) ); + } private SendableChooser getTestCommandChooser() {