Trigger LEDs at start of note lift sequence, disable lock to line

frc868 · Mar 22, 2024 · f074f4f · f074f4f
1 parent 0077b40
commit f074f4f
Showing 1 changed file with 2 additions and 6 deletions.
diff --git a/src/main/java/frc/robot/RobotCommands.java b/src/main/java/frc/robot/RobotCommands.java
@@ -122,10 +122,10 @@ public static Command shootOnTheMoveCommand(Drivetrain drivetrain, Intake intake
     public static Command intakeToNoteLift(Shooter shooter, ShooterTilt shooterTilt, NoteLift noteLift, LEDs leds) {
         // proxying to allow shooterTilt to hold position while note lift moves down
         return Commands.sequence(
+                new ScheduleCommand(leds.requestStateCommand(LEDState.FLASHING_WHITE).withTimeout(5)),
                 new ScheduleCommand(shooter.stopCommand()),
                 shooterTilt.moveToPositionCommand(() -> ShooterTiltPosition.CLIMB).asProxy(),
-                noteLift.moveToPositionCommand(() -> NoteLiftPosition.INTAKE).asProxy(),
-                leds.requestStateCommand(LEDState.FLASHING_WHITE));
+                noteLift.moveToPositionCommand(() -> NoteLiftPosition.INTAKE).asProxy());
     }
 
     public static Command prepareClimb(DoubleSupplier xSpeedSupplier, DoubleSupplier ySpeedSupplier,
@@ -134,10 +134,6 @@ public static Command prepareClimb(DoubleSupplier xSpeedSupplier, DoubleSupplier
             NoteLift noteLift) {
         return Commands.parallel(
                 new ScheduleCommand(shooter.stopCommand()),
-                drivetrain.targetStageCommand(xSpeedSupplier, ySpeedSupplier)
-                        .unless(GlobalStates.DRIVETRAIN_TARGETTING_DISABLED::enabled)
-                        .until(GlobalStates.DRIVETRAIN_TARGETTING_DISABLED::enabled)
-                        .andThen(drivetrain.teleopDriveCommand(xSpeedSupplier, ySpeedSupplier, thetaSpeedSupplier)),
                 shooterTilt.moveToPositionCommand(() -> ShooterTiltPosition.CLIMB).asProxy(),
                 intake.moveToPositionCommand(() -> IntakePosition.GROUND).asProxy(),
                 noteLift.moveToPositionCommand(() -> NoteLiftPosition.CLIMB_PREP).asProxy(),