diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1b6c243..ad56eb3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -140,6 +140,12 @@ public class RobotContainer { controllerlower.rightTrigger() .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingBump)) .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); + + controllerlower.a() + .onTrue(robotStatusManager.setStatusCommand(RobotStatus.Climbing)) + .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); + + } private void configueShooter(){ diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index b273d0a..7958cbd 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -145,6 +145,7 @@ public class DriveControls { autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN); break; case Climbing: + drivetrain.driveToAprilTag(); break; case CrossingBump: double differenceBump = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.bumpY[0], Constants.AutoPositioning.bumpY[1]);