Bump Autoalign

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---

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);