diff --git a/src/main/java/org/littletonrobotics/frc2024/Robot.java b/src/main/java/org/littletonrobotics/frc2024/Robot.java index 0122bc74..15359dc3 100644 --- a/src/main/java/org/littletonrobotics/frc2024/Robot.java +++ b/src/main/java/org/littletonrobotics/frc2024/Robot.java @@ -28,6 +28,7 @@ import java.util.HashMap; import java.util.Map; import java.util.function.BiConsumer; +import java.util.function.DoubleSupplier; import org.littletonrobotics.frc2024.Constants.Mode; import org.littletonrobotics.frc2024.subsystems.leds.Leds; import org.littletonrobotics.frc2024.util.Alert; @@ -83,12 +84,12 @@ public class Robot extends LoggedRobot { private final Alert sameBatteryAlert = new Alert("The battery has not been changed since the last match.", AlertType.WARNING); - public static Trigger createTeleopTimeTrigger(double teleElapsedTime) { + public static Trigger createTeleopTimeTrigger(DoubleSupplier teleElapsedTime) { return new Trigger( () -> DriverStation.isFMSAttached() && DriverStation.isTeleopEnabled() - && Robot.teleElapsedTime > teleElapsedTime); + && Robot.teleElapsedTime > teleElapsedTime.getAsDouble()); } /** diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java b/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java index 6ece0e03..db3da1d3 100644 --- a/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java +++ b/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.WrapperCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; import lombok.experimental.ExtensionMethod; +import org.littletonrobotics.frc2024.Robot; import org.littletonrobotics.frc2024.RobotState; import org.littletonrobotics.frc2024.subsystems.drive.Drive; import org.littletonrobotics.frc2024.subsystems.leds.Leds; @@ -26,6 +27,8 @@ public class ClimbingCommands { private static final LoggedTunableNumber chainToBack = new LoggedTunableNumber("ClimbingCommands/ChainToBackOffset", 0.3); + private static final LoggedTunableNumber autoUntrapTime = + new LoggedTunableNumber("ClimbingCommands/AutoUntrapTime", 134.5); public static Command trapSequence( Drive drive, @@ -33,9 +36,16 @@ public static Command trapSequence( Rollers rollers, Trigger forwardTrigger, Trigger reverseTrigger) { + Trigger endOfMatch = Robot.createTeleopTimeTrigger(autoUntrapTime); SteppableCommandGroup sequence = new SteppableCommandGroup( - forwardTrigger.and(superstructure::atGoal), + forwardTrigger + .and(superstructure::atArmGoal) + .or( + endOfMatch.and( + () -> + superstructure.getDesiredGoal() == Superstructure.Goal.TRAP + && rollers.getGoal() == Rollers.Goal.TRAP_SCORE)), reverseTrigger, // Move arm to prepare prepare climb setpoint while moving climbers up @@ -57,15 +67,16 @@ public static Command trapSequence( .alongWith( superstructure.setGoalWithConstraintsCommand( Superstructure.Goal.PREPARE_CLIMB, - Arm.prepareClimbProfileConstraints.get())), + Arm.prepareClimbProfileConstraints.get()), + rollers.setGoalCommand(Rollers.Goal.SHUFFLE_BACKPACK)), // Allow driver to line up and climb - superstructure - .setGoalCommand(Superstructure.Goal.CLIMB) - .alongWith(rollers.setGoalCommand(Rollers.Goal.SHUFFLE_BACKPACK)), + superstructure.setGoalCommand(Superstructure.Goal.CLIMB), // Extend backpack - superstructure.setGoalCommand(Superstructure.Goal.TRAP), + superstructure + .setGoalCommand(Superstructure.Goal.TRAP) + .alongWith(rollers.setGoalCommand(Rollers.Goal.TRAP_PRESCORE)), // Trap. superstructure