From 72cb73b727dec75c7e8abe160342f662dcf99748 Mon Sep 17 00:00:00 2001 From: Surya Thoppae <63197592+suryatho@users.noreply.github.com> Date: Thu, 14 Mar 2024 18:13:40 -0400 Subject: [PATCH] Add climber sequencing to driver controller (#204) * add trap and simple climb sequences * .. * Add name for teleop drive * Check style * Fix typo * Finish climb sequence --------- Co-authored-by: Jonah <47046556+jwbonner@users.noreply.github.com> --- .../org/littletonrobotics/frc2024/Robot.java | 5 +- .../frc2024/RobotContainer.java | 57 +-- .../frc2024/commands/ClimbingCommands.java | 326 ++++++------------ 3 files changed, 123 insertions(+), 265 deletions(-) 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/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 09399bc8..4c238f11 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -19,7 +19,6 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -122,7 +121,6 @@ public class RobotContainer { new LoggedDashboardNumber("Endgame Alert #2", 15.0); private boolean podiumShotMode = false; - private boolean trapScoreMode = true; private boolean armCoastOverride = false; // Dashboard inputs @@ -415,7 +413,6 @@ private void configureAutos() { */ private void configureButtonBindings() { // ------------- Driver Controls ------------- - // Drive command drive.setDefaultCommand( drive .run( @@ -632,17 +629,22 @@ private void configureButtonBindings() { .whileTrue(rollers.setGoalCommand(Rollers.Goal.AMP_SCORE).onlyWhile(driver.rightTrigger())); // ------------- Climbing Controls ------------- + Command trapSequence = + ClimbingCommands.trapSequence( + drive, superstructure, rollers, driver.povUp(), driver.povDown()); + Command simpleSequence = + ClimbingCommands.simpleSequence(superstructure, driver.povUp(), driver.povDown()); driver - .x() - .and( - () -> - superstructure.getCurrentGoal() != Superstructure.Goal.CANCEL_CLIMB - && superstructure.getCurrentGoal() != Superstructure.Goal.CANCEL_PREPARE_CLIMB) - .toggleOnTrue( - Commands.either( - superstructure.setGoalCommand(Superstructure.Goal.PREPARE_PREPARE_TRAP_CLIMB), - Commands.none(), // No function yet - () -> trapScoreMode)); + .start() + .and(driver.back()) + .onTrue( + Commands.runOnce( + () -> { + simpleSequence.cancel(); + trapSequence.cancel(); + })); + driver.x().doublePress().onTrue(trapSequence); + driver.y().doublePress().onTrue(simpleSequence); // ------------- Operator Controls ------------- // Adjust shot compensation @@ -669,34 +671,6 @@ private void configureButtonBindings() { .a() .onTrue(Commands.runOnce(() -> podiumShotMode = false)); // set preset mode to subwoofer - // Climber controls - operator.rightStick().onTrue(Commands.runOnce(() -> trapScoreMode = !trapScoreMode)); - operator - .leftBumper() - .doublePress() - .and(() -> trapScoreMode) - .toggleOnTrue( - ClimbingCommands.climbNTrapSequence( - drive, - superstructure, - rollers, - () -> -driver.getLeftY(), - () -> -driver.getLeftX(), - () -> -driver.getRightX(), - operator.rightBumper().doublePress(), - operator.start().doublePress().or(operator.back().doublePress()), - autoDriveDisable) - .withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - operator - .leftBumper() - .doublePress() - .and(() -> !trapScoreMode) - .toggleOnTrue( - ClimbingCommands.simpleClimbSequence( - superstructure, operator.rightBumper().doublePress()) - .withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - operator.leftStick().onTrue(superstructure.setGoalCommand(Superstructure.Goal.RESET)); - // Request amp operator .x() @@ -769,7 +743,6 @@ public void updateDashboardOutputs() { "Shot Compensation Degrees", String.format("%.1f", robotState.getShotCompensationDegrees())); SmartDashboard.putBoolean("Podium Preset", podiumShotMode); - SmartDashboard.putBoolean("Trap Score Mode", trapScoreMode); SmartDashboard.putNumber("Match Time", DriverStation.getMatchTime()); } diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java b/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java index ecb05a64..db3da1d3 100644 --- a/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java +++ b/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java @@ -7,250 +7,134 @@ package org.littletonrobotics.frc2024.commands; -import static org.littletonrobotics.frc2024.FieldConstants.*; - import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.WrapperCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; -import java.util.List; -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; import lombok.experimental.ExtensionMethod; -import org.littletonrobotics.frc2024.FudgeFactors; +import org.littletonrobotics.frc2024.Robot; import org.littletonrobotics.frc2024.RobotState; import org.littletonrobotics.frc2024.subsystems.drive.Drive; +import org.littletonrobotics.frc2024.subsystems.leds.Leds; import org.littletonrobotics.frc2024.subsystems.rollers.Rollers; import org.littletonrobotics.frc2024.subsystems.superstructure.Superstructure; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm; -import org.littletonrobotics.frc2024.util.AllianceFlipUtil; import org.littletonrobotics.frc2024.util.GeomUtil; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; +import org.littletonrobotics.frc2024.util.SteppableCommandGroup; @ExtensionMethod({GeomUtil.class}) public class ClimbingCommands { - private static final LoggedTunableNumber climbedXOffset = - new LoggedTunableNumber("ClimbingCommands/ClimbedXOffset", 0.16); private static final LoggedTunableNumber chainToBack = new LoggedTunableNumber("ClimbingCommands/ChainToBackOffset", 0.3); - private static final LoggedTunableNumber chainToFront = - new LoggedTunableNumber("ClimbingCommands/ChainToFrontOffset", 0.9); - - private static final List> centeredClimbedPosesNoOffset = - List.of( - () -> - Stage.centerPodiumAmpChain.transformBy( - FudgeFactors.centerPodiumAmpChain.getTransform()), - () -> - Stage.centerAmpSourceChain.transformBy( - FudgeFactors.centerAmpSourceChain.getTransform()), - () -> - Stage.centerSourcePodiumChain.transformBy( - FudgeFactors.centerSourcePodiumChain.getTransform())); - - private static final Supplier nearestClimbedPose = - () -> { - Pose2d currentPose = RobotState.getInstance().getEstimatedPose(); - List climbedPoses = - centeredClimbedPosesNoOffset.stream() - .map(Supplier::get) - .map(AllianceFlipUtil::apply) - .toList(); - return currentPose.nearest(climbedPoses); - }; - private static final Supplier backPrepareClimbPose = - () -> - nearestClimbedPose - .get() - .transformBy( - new Translation2d(climbedXOffset.get() - chainToBack.get(), 0).toTransform2d()); - private static final Supplier frontPrepareClimbPose = - () -> - nearestClimbedPose - .get() - .transformBy( - new Translation2d(-climbedXOffset.get() + chainToFront.get(), 0).toTransform2d()) - .transformBy( - new Transform2d( - 0, 0, Rotation2d.fromDegrees(180.0))); // Flip climbed pose for front climb - - /** Command that lets driver adjust robot relative to the robot at slow speed */ - private static Command driverAdjust( - Drive drive, - DoubleSupplier controllerX, - DoubleSupplier controllerY, - DoubleSupplier controllerOmega) { - return drive.run( - () -> - drive.acceptTeleopInput( - controllerX.getAsDouble(), - controllerY.getAsDouble(), - controllerOmega.getAsDouble(), - false)); - } - - private static Command driveToPoseWithAdjust( - Drive drive, - Supplier prepareClimbPose, - DoubleSupplier controllerX, - DoubleSupplier controllerY) { - return Commands.sequence( - // Auto drive to behind the chain - drive - .startEnd( - () -> drive.setAutoAlignGoal(prepareClimbPose, false), drive::clearAutoAlignGoal) - .until(drive::isAutoAlignGoalCompleted), - - // Let driver move robot left and right while aligned to chain - driverAdjust(drive, controllerX, controllerY, () -> 0.0)); - } + private static final LoggedTunableNumber autoUntrapTime = + new LoggedTunableNumber("ClimbingCommands/AutoUntrapTime", 134.5); - /** Drive to back climber ready pose. */ - public static Command autoDrive( - boolean isFront, - Drive drive, - DoubleSupplier controllerX, - DoubleSupplier controllerY, - Trigger autoDriveDisable) { - return driveToPoseWithAdjust( - drive, isFront ? frontPrepareClimbPose : backPrepareClimbPose, controllerX, controllerY) - .onlyIf(autoDriveDisable.negate()); - } - - /** - * Drives to climbed pose while raising climber up and arm back, ends when at position with drive - * and superstructure. - */ - private static Command prepareClimbFromBack( - Drive drive, Superstructure superstructure, Trigger autoDriveDisable) { - return drive - .startEnd( - () -> { - Pose2d currentPose = RobotState.getInstance().getEstimatedPose(); - Pose2d targetPose = - currentPose.transformBy(new Translation2d(chainToBack.get(), 0).toTransform2d()); - drive.setAutoAlignGoal(() -> targetPose, true); - }, - drive::clearAutoAlignGoal) - .onlyIf(autoDriveDisable.negate()) - .alongWith( - superstructure.setGoalWithConstraintsCommand( - Superstructure.Goal.PREPARE_CLIMB, Arm.prepareClimbProfileConstraints.get())); - } - - private static Command prepareClimbFromFront( - Drive drive, Superstructure superstructure, Trigger autoDriveDisable) { - return drive - .startEnd( - () -> { - Pose2d currentPose = RobotState.getInstance().getEstimatedPose(); - Pose2d targetPose = - currentPose.transformBy(new Translation2d(chainToFront.get(), 0).toTransform2d()); - drive.setAutoAlignGoal(() -> targetPose, true); - }, - drive::clearAutoAlignGoal) - .onlyIf(autoDriveDisable.negate()) - .alongWith(superstructure.setGoalCommand(Superstructure.Goal.PREPARE_CLIMB)); - } - - public static Command simpleClimbSequence( - Superstructure superstructure, Trigger startClimbTrigger) { - return superstructure - .setGoalCommand(Superstructure.Goal.PREPARE_CLIMB) - .until(startClimbTrigger) - .andThen(superstructure.setGoalCommand(Superstructure.Goal.CLIMB)) - - // If cancelled, go to safe state - .finallyDo( - () -> { - switch (superstructure.getCurrentGoal()) { - case PREPARE_CLIMB, CANCEL_PREPARE_CLIMB -> - superstructure.setDefaultCommand( - superstructure.setGoalCommand(Superstructure.Goal.CANCEL_PREPARE_CLIMB)); - case CLIMB, CANCEL_CLIMB -> - superstructure.setDefaultCommand( - superstructure.setGoalCommand(Superstructure.Goal.CANCEL_CLIMB)); - } - }); - } - - /** Runs the climbing sequence and then scores in the trap when the trapScore button is pressed */ - public static Command climbNTrapSequence( + public static Command trapSequence( Drive drive, Superstructure superstructure, Rollers rollers, - DoubleSupplier controllerX, - DoubleSupplier controllerY, - DoubleSupplier controllerOmega, - Trigger startClimbTrigger, - Trigger trapScoreTrigger, - Trigger autoDriveDisable) { - return Commands.sequence( - // Drive forward while raising arm and climber - prepareClimbFromBack(drive, superstructure, autoDriveDisable) - .until(() -> superstructure.atGoal() && drive.isAutoAlignGoalCompleted()) - .withTimeout(5.0), + Trigger forwardTrigger, + Trigger reverseTrigger) { + Trigger endOfMatch = Robot.createTeleopTimeTrigger(autoUntrapTime); + SteppableCommandGroup sequence = + new SteppableCommandGroup( + 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 + superstructure.setGoalCommand(Superstructure.Goal.PREPARE_PREPARE_TRAP_CLIMB), - // Allow driver to line up - Commands.waitUntil(startClimbTrigger) - .deadlineWith( - superstructure.setGoalCommand(Superstructure.Goal.PREPARE_CLIMB), - driverAdjust(drive, controllerX, controllerY, controllerOmega)), - - // Climb and wait, continue if trap button pressed + // Drive forward while raising arm and climber + drive + .startEnd( + () -> { + Pose2d robot = RobotState.getInstance().getEstimatedPose(); + Pose2d target = + robot.transformBy(GeomUtil.toTransform2d(chainToBack.get(), 0.0)); + drive.setAutoAlignGoal(() -> target, true); + }, + drive::clearAutoAlignGoal) + .asProxy() + .until(() -> drive.isAutoAlignGoalCompleted() && superstructure.atGoal()) + .withTimeout(5.0) + .alongWith( + superstructure.setGoalWithConstraintsCommand( + Superstructure.Goal.PREPARE_CLIMB, + Arm.prepareClimbProfileConstraints.get()), + rollers.setGoalCommand(Rollers.Goal.SHUFFLE_BACKPACK)), + + // Allow driver to line up and climb + superstructure.setGoalCommand(Superstructure.Goal.CLIMB), + + // Extend backpack superstructure - .setGoalCommand(Superstructure.Goal.CLIMB) - .alongWith(rollers.setGoalCommand(Rollers.Goal.SHUFFLE_BACKPACK)) - .raceWith( - Commands.waitUntil(trapScoreTrigger) - .andThen(Commands.waitUntil(trapScoreTrigger.negate()))), - - // Repeat trap sequence forever and ever - Commands.sequence( - // Extend backpack - superstructure - .setGoalCommand(Superstructure.Goal.TRAP) - .alongWith(rollers.setGoalCommand(Rollers.Goal.TRAP_PRESCORE)) - .raceWith( - Commands.waitUntil(trapScoreTrigger) - .andThen(Commands.waitUntil(trapScoreTrigger.negate()))), + .setGoalCommand(Superstructure.Goal.TRAP) + .alongWith(rollers.setGoalCommand(Rollers.Goal.TRAP_PRESCORE)), - // Score in trap and wait - rollers - .setGoalCommand(Rollers.Goal.TRAP_SCORE) - .alongWith(superstructure.setGoalCommand(Superstructure.Goal.TRAP)) - .raceWith( - Commands.waitUntil(trapScoreTrigger) - .andThen(Commands.waitUntil(trapScoreTrigger.negate()))), - - // Retract backpack - superstructure - .setGoalCommand(Superstructure.Goal.UNTRAP) - .raceWith( - Commands.waitUntil(trapScoreTrigger) - .andThen(Commands.waitUntil(trapScoreTrigger.negate())))) - .repeatedly()) + // Trap. + superstructure + .setGoalCommand(Superstructure.Goal.TRAP) + .alongWith(rollers.setGoalCommand(Rollers.Goal.TRAP_SCORE)), + + // Untrap + superstructure.setGoalCommand(Superstructure.Goal.UNTRAP)); + return new WrapperCommand( + sequence + .beforeStarting(() -> Leds.getInstance().climbing = true) + .finallyDo(() -> Leds.getInstance().climbing = false)) { + @Override + public InterruptionBehavior getInterruptionBehavior() { + if (sequence.getCurrentCommandIndex().isPresent() + && sequence.getCurrentCommandIndex().getAsInt() == 0) { + return InterruptionBehavior.kCancelSelf; + } + return InterruptionBehavior.kCancelIncoming; + } + + @Override + public String getName() { + return "Trap Climb"; + } + }; + } - // If cancelled, go to safe state - .finallyDo( - () -> { - switch (superstructure.getCurrentGoal()) { - case PREPARE_CLIMB, CANCEL_PREPARE_CLIMB -> - superstructure.setDefaultCommand( - superstructure.setGoalCommand(Superstructure.Goal.CANCEL_PREPARE_CLIMB)); - case CLIMB, CANCEL_CLIMB -> - superstructure.setDefaultCommand( - superstructure.setGoalCommand(Superstructure.Goal.CANCEL_CLIMB)); - case TRAP -> - superstructure.setDefaultCommand( - superstructure.setGoalCommand(Superstructure.Goal.CLIMB)); - case UNTRAP -> - superstructure.setDefaultCommand( - superstructure.setGoalCommand(Superstructure.Goal.CLIMB)); - } - }); + public static Command simpleSequence( + Superstructure superstructure, Trigger forwardTrigger, Trigger reverseTrigger) { + SteppableCommandGroup sequence = + new SteppableCommandGroup( + forwardTrigger.and(superstructure::atGoal), + reverseTrigger, + + // Raise climber + superstructure.setGoalCommand(Superstructure.Goal.PREPARE_CLIMB), + + // Climb + superstructure.setGoalCommand(Superstructure.Goal.CLIMB)); + return new WrapperCommand( + sequence + .beforeStarting(() -> Leds.getInstance().climbing = true) + .finallyDo(() -> Leds.getInstance().climbing = false)) { + + @Override + public InterruptionBehavior getInterruptionBehavior() { + if (sequence.getCurrentCommandIndex().isPresent() + && sequence.getCurrentCommandIndex().getAsInt() == 0) { + return InterruptionBehavior.kCancelSelf; + } + return InterruptionBehavior.kCancelIncoming; + } + + @Override + public String getName() { + return "Simple Climb"; + } + }; } }