From 30ee3c6293834ba3ec8dcf647d16b9a00790918f Mon Sep 17 00:00:00 2001
From: NandaGuntupalli <70027620+NandaGuntupalli@users.noreply.github.com>
Date: Fri, 20 Sep 2024 09:03:39 -0400
Subject: [PATCH] Created Geomapping

---
 src/main/java/frc/robot/Constants.java        |  1 +
 src/main/java/frc/robot/RobotContainer.java   | 26 +++++++++++++++++++
 .../java/frc/robot/subsystems/Swerve.java     | 12 +++++++++
 3 files changed, 39 insertions(+)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index d3ae55e..7b8ae0c 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -471,6 +471,7 @@ public static final class SwerveConstants {
     public static final double maxTranslationalSpeed = Units.feetToMeters(14.5);
     public static final double maxAngularSpeed = Units.degreesToRadians(180);
     public static final double slowMotionMaxTranslationalSpeed = Units.feetToMeters(3.53);
+    public static final double bufferMaxTranslationalSpeed = Units.feetToMeters(1);
     public static final double turboMaxTranslationalSpeed = Units.feetToMeters(14.5);
 
     public static final double maxTranslationalAcceleration = Units.feetToMeters(18.0);
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 701f367..da956eb 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -7,6 +7,7 @@
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -89,6 +90,10 @@ public class RobotContainer implements Logged {
   private Alert swervePrematchAlert = new Alert("", AlertType.INFO);
   private Alert generalPrematchAlert = new Alert("", AlertType.INFO);
 
+  Pose2d currentPose = swerve.getPose();
+  double xCoordinate = currentPose.getX();
+  double yCoordinate = currentPose.getY();
+
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     DataLogManager.log("Initializing Robot Container");
@@ -349,6 +354,27 @@ private void configureDriveBindings() {
 
     driverController.x().whileTrue(swerve.run(swerve::lockModules));
 
+    if (xCoordinate > 7.5 || xCoordinate < -7.5 || yCoordinate > 5 || yCoordinate < -5) {
+      driverController.rumbleFor(5, RumbleType.kBothRumble, 1);
+    }
+
+    driverController.b().onTrue(
+      new TeleopSwerve(
+            driverController::getLeftY,
+            driverController::getLeftX,
+            driverController::getRightX,
+            driverController::getRightY,
+            driverController::getLeftBumper,
+            () -> {
+              if (xCoordinate > 7.5 || xCoordinate < -7.5 || yCoordinate > 5 || yCoordinate < -5) {
+                return SwerveConstants.bufferMaxTranslationalSpeed;
+              }
+              return SwerveConstants.maxTranslationalSpeed;
+            },
+            SwerveConstants.maxAngularSpeed,
+            swerve)
+    );
+
     // slowMode
     //     .and(turnToSpeaker.negate())
     //     .whileTrue(
diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java
index 46ce4ed..522a802 100644
--- a/src/main/java/frc/robot/subsystems/Swerve.java
+++ b/src/main/java/frc/robot/subsystems/Swerve.java
@@ -618,6 +618,18 @@ public SwerveModuleState[] getDesiredStates() {
     };
   }
 
+  Pose2d currentPose = getPose();
+  double xCoordinate = currentPose.getX();
+  double yCoordinate = currentPose.getY();
+
+  public boolean isPracticeOutOfBounds() {
+    if (xCoordinate > 7.5 || xCoordinate < -7.5 || yCoordinate > 5 || yCoordinate < -5) {
+      return true;
+    } else {
+      return false;
+    }
+  }
+
   private boolean odometryUpdateValid(Twist2d delta) {
     Pose2d poseExponential = new Pose2d().exp(delta);