diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java
index 03e5a0240..be0025fff 100644
--- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java
+++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java
@@ -262,7 +262,10 @@ public void initialize() {
       }
     }
 
-    if (currentPose.getTranslation().getDistance(targetPose.getTranslation()) < 0.5) {
+    if (currentPose.getTranslation().getDistance(targetPose.getTranslation())
+        < (Pathfinding.getNavgridSize() * Math.sqrt(2))) {
+      // This finds the max distance in your current navgrid and makes sure that it cannot pathfind
+      // to itself.
       output.accept(new ChassisSpeeds(), DriveFeedforwards.zeros(robotConfig.numModules));
       finish = true;
     } else {
@@ -289,7 +292,7 @@ public void execute() {
             && currentPose
                     .getTranslation()
                     .getDistance(currentTrajectory.getEndState().pose.getTranslation())
-                < 2.0;
+                < (Pathfinding.getNavgridSize()*4);
 
     if (!skipUpdates && Pathfinding.isNewPathAvailable()) {
       currentPath = Pathfinding.getCurrentPath(constraints, goalEndState);
diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/Pathfinding.java b/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/Pathfinding.java
index f5a2a74f2..85b5864ac 100644
--- a/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/Pathfinding.java
+++ b/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/Pathfinding.java
@@ -5,7 +5,13 @@
 import com.pathplanner.lib.path.PathPlannerPath;
 import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.Filesystem;
+import java.io.BufferedReader;
+import java.io.File;
+import java.io.FileReader;
 import java.util.List;
+import org.json.simple.JSONObject;
+import org.json.simple.parser.JSONParser;
 
 /**
  * Static class for interacting with the chosen pathfinding implementation from the pathfinding
@@ -22,6 +28,32 @@ public class Pathfinding {
   public static void setPathfinder(Pathfinder pathfinder) {
     Pathfinding.pathfinder = pathfinder;
   }
+   /**
+   * Get the current navgrid size from the navgrid.json file in deploy
+   *
+   * @return Navgrid size (double)
+   */
+  public static double getNavgridSize() {
+    File navGridFile = new File(Filesystem.getDeployDirectory(), "pathplanner/navgrid.json");
+    if (navGridFile.exists()) {
+      try (BufferedReader br = new BufferedReader(new FileReader(navGridFile))) {
+        StringBuilder fileContentBuilder = new StringBuilder();
+        String line;
+        while ((line = br.readLine()) != null) {
+          fileContentBuilder.append(line);
+        }
+
+        String fileContent = fileContentBuilder.toString();
+        JSONObject json = (JSONObject) new JSONParser().parse(fileContent);
+
+        return ((Number) json.get("nodeSizeMeters")).doubleValue();
+
+      } catch (Exception e) {
+        return 0.3;
+      }
+    }
+    return 0.3;
+  }
 
   /** Ensure that a pathfinding implementation has been chosen. If not, set it to the default. */
   public static void ensureInitialized() {