diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 1fea6bd4..33458c25 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -546,18 +546,8 @@ private void configureButtonBindings() { && superstructure.getCurrentGoal() != Superstructure.Goal.CANCEL_PREPARE_CLIMB) .whileTrue( Commands.either( - ClimbingCommands.autoDrive( - false, - drive, - () -> -driver.getLeftY(), - () -> -driver.getLeftX(), - autoDriveDisable), - ClimbingCommands.autoDrive( - true, - drive, - () -> -driver.getLeftY(), - () -> -driver.getLeftX(), - autoDriveDisable), + superstructure.setGoalCommand(Superstructure.Goal.PREPARE_PREPARE_TRAP_CLIMB), + Commands.none(), () -> trapScoreMode)); // ------------- Operator Controls ------------- diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java b/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java index db59e2bf..ee25504f 100644 --- a/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java +++ b/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java @@ -34,7 +34,7 @@ public class ClimbingCommands { private static final LoggedTunableNumber climbedXOffset = new LoggedTunableNumber("ClimbingCommands/ClimbedXOffset", 0.16); private static final LoggedTunableNumber chainToBack = - new LoggedTunableNumber("ClimbingCommands/ChainToBackOffset", 0.65); + new LoggedTunableNumber("ClimbingCommands/ChainToBackOffset", 0.3); private static final LoggedTunableNumber chainToFront = new LoggedTunableNumber("ClimbingCommands/ChainToFrontOffset", 0.9); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java index d0859260..79bc98f3 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java @@ -31,6 +31,7 @@ public enum Goal { SUBWOOFER, PODIUM, RESET_CLIMB, + PREPARE_PREPARE_TRAP_CLIMB, PREPARE_CLIMB, CANCEL_PREPARE_CLIMB, CLIMB, @@ -123,6 +124,11 @@ public void periodic() { climber.setGoal(Climber.Goal.IDLE); backpackActuator.setGoal(BackpackActuator.Goal.RETRACT); } + case PREPARE_PREPARE_TRAP_CLIMB -> { + arm.setGoal(Arm.Goal.PREPARE_PREPARE_TRAP_CLIMB); + climber.setGoal(Climber.Goal.EXTEND); + backpackActuator.setGoal(BackpackActuator.Goal.RETRACT); + } case PREPARE_CLIMB -> { arm.setGoal(Arm.Goal.PREPARE_CLIMB); climber.setGoal(Climber.Goal.EXTEND); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java index 955f27b7..8c6dcc4c 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java @@ -81,6 +81,7 @@ public enum Goal { AMP(new LoggedTunableNumber("Arm/AmpDegrees", 110.0)), SUBWOOFER(new LoggedTunableNumber("Arm/SubwooferDegrees", 55.0)), PODIUM(new LoggedTunableNumber("Arm/PodiumDegrees", 30.0)), + PREPARE_PREPARE_TRAP_CLIMB(new LoggedTunableNumber("Arm/PreparePrepareTrapClimbDegrees", 40.0)), PREPARE_CLIMB(new LoggedTunableNumber("Arm/PrepareClimbDegrees", 105.0)), CLIMB(new LoggedTunableNumber("Arm/ClimbDegrees", 90.0)), CUSTOM(new LoggedTunableNumber("Arm/CustomSetpoint", 20.0));