diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d6d290e..1635d05 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -435,6 +435,17 @@ private void addControllerBindings() { () -> modifyJoystick(driver.getLeftX()) * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed())); + + while(swerve.isCloseToBump()){ + swerve.bumpAlign( + () -> + modifyJoystick(driver.getLeftY()) + * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), + () -> + modifyJoystick(driver.getLeftX()) + * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed()); + } + // TODO add binding for climb // ---zeroing stuff--- diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index e422313..b856f64 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -605,6 +605,31 @@ public Command faceHub(DoubleSupplier xVel, DoubleSupplier yVel) { yVel); } + public Command bumpAlign(DoubleSupplier xVel, DoubleSupplier yVel) { + return driveWithHeadingSnap( + () -> { + if(getPose().getRotation().getDegrees() <= 90 || getPose().getRotation().getDegrees() >= 270){ + return Rotation2d.kZero; + } else { + return Rotation2d.k180deg; + } + }, + xVel, + yVel); + } + + public boolean isCloseToBump(){ + if( + ((Math.abs(getPose().getX() - 4.62) < 2) || (Math.abs(getPose().getX() - 11.91) < 2)) + && + ((getPose().getY() > 5.04 && getPose().getY() < 6.07) || (getPose().getY() > 2 && getPose().getY() < 3.01))) + { + return true; + } else { + return false; + } + } + public boolean isInAutoAimTolerance(Pose2d target) { return isInTolerance( target, AutoAlign.TRANSLATION_TOLERANCE_METERS, AutoAlign.ROTATION_TOLERANCE_RADIANS);