diff --git a/build.gradle b/build.gradle
index 81c0f3b..c5ce1a7 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2025.1.1"
+    id "edu.wpi.first.GradleRIO" version "2025.2.1"
 }
 
 java {
@@ -33,7 +33,7 @@ deploy {
                 frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
                     files = project.fileTree('src/main/deploy')
                     directory = '/home/lvuser/deploy'
-                    deleteOldFiles = false // Change to true to delete files on roboRIO that no
+                    deleteOldFiles = true // Change to true to delete files on roboRIO that no
                                            // longer exist in deploy directory of this project
                 }
             }
diff --git a/simgui-ds.json b/simgui-ds.json
new file mode 100644
index 0000000..73cc713
--- /dev/null
+++ b/simgui-ds.json
@@ -0,0 +1,92 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ]
+}
diff --git a/src/main/deploy/pathplanner/autos/CoolAuto.auto b/src/main/deploy/pathplanner/autos/CoolAuto.auto
new file mode 100644
index 0000000..70b7ab2
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/CoolAuto.auto
@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Example Path"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
new file mode 100644
index 0000000..23e0db9
--- /dev/null
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -0,0 +1 @@
+{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
new file mode 100644
index 0000000..afb9553
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Example Path.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.1123076923076916,
+        "y": 7.0
+      },
+      "prevControl": {
+        "x": 5.1123076923076916,
+        "y": 7.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
new file mode 100644
index 0000000..c64d29d
--- /dev/null
+++ b/src/main/deploy/pathplanner/settings.json
@@ -0,0 +1,34 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": true,
+  "pathFolders": [
+    "New Folder"
+  ],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "defaultNominalVoltage": 12.0,
+  "robotMass": 74.088,
+  "robotMOI": 6.883,
+  "robotTrackwidth": 0.546,
+  "driveWheelRadius": 0.048,
+  "driveGearing": 5.143,
+  "maxDriveSpeed": 5.45,
+  "driveMotorType": "krakenX60",
+  "driveCurrentLimit": 60.0,
+  "wheelCOF": 1.2,
+  "flModuleX": 0.273,
+  "flModuleY": 0.273,
+  "frModuleX": 0.273,
+  "frModuleY": -0.273,
+  "blModuleX": -0.273,
+  "blModuleY": 0.273,
+  "brModuleX": -0.273,
+  "brModuleY": -0.273,
+  "bumperOffsetX": 0.0,
+  "bumperOffsetY": 0.0,
+  "robotFeatures": []
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 91aa6a3..37d9337 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -19,6 +19,38 @@ public static class OperatorConstants {
 
   public static class DrivetrainConstants {
     public static final int kLeftMotorCANID = 10;
+    public static final int kOptionalLeftMotorCANID = 11;
+
     public static final int kRightMotorCANID = 20;
+    public static final int kOptionalRightMotorCANID = 21;
+
+    public static final double kTurnDivider = 2;
+    public static final double kSpeedDivider = 2.5;
+
+    // Auto PID stuff
+    public static final double kV = 0; // Add x V output to overcome static friction
+    public static final double kS = 0; // A velocity target of 1 rps results in xV output
+    public static final double kP = 0.3; // An error of 1 rotation results in x V output
+    public static final double kI = 0.0;
+    public static final double kD = 0.1; // A velocity of 1 rps results in x V output
+    public static final double PeakVoltage = 10.0;
+
+    public static final int maxVelocity = 30; // rps/s
+    public static final int maxAcceleration = 50; // rps
+
+    // For Auto Potentially
+    public static boolean kLeftPositiveMovesForward = true;
+    public static boolean kRightPositiveMovesForward = true;
+
+    // The distance travelled for a single rotation of the Kraken output shaft.
+    public static final double kDrivetrainGearRatio = (8.46/0.478536);
+    public static final double kWheelRadiusMeters = (4.0 / 2.0) * 0.0254; // Four Inch Wheels
+    public static final double kWheelCircumfrance = 2 * Math.PI * DrivetrainConstants.kWheelRadiusMeters;
+
+    // SmartDashboard update frequency for drive subsystem state in 20ms counts.
+    public static final int kTicksPerUpdate = 5;
+
+    // The track width in meters.
+    public static final double trackWidthMeters = 1; // TODO: Set Value!
   }
 }
diff --git a/src/main/java/frc/robot/LimelightHelpers.java b/src/main/java/frc/robot/LimelightHelpers.java
new file mode 100644
index 0000000..854968f
--- /dev/null
+++ b/src/main/java/frc/robot/LimelightHelpers.java
@@ -0,0 +1,1647 @@
+//LimelightHelpers v1.11 (REQUIRES LLOS 2025.0 OR LATER)
+
+package frc.robot;
+
+import edu.wpi.first.networktables.DoubleArrayEntry;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.TimestampedDoubleArray;
+import frc.robot.LimelightHelpers.LimelightResults;
+import frc.robot.LimelightHelpers.PoseEstimate;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+import java.io.IOException;
+import java.net.HttpURLConnection;
+import java.net.MalformedURLException;
+import java.net.URL;
+import java.util.Map;
+import java.util.concurrent.CompletableFuture;
+
+import com.fasterxml.jackson.annotation.JsonFormat;
+import com.fasterxml.jackson.annotation.JsonFormat.Shape;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.DeserializationFeature;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import java.util.concurrent.ConcurrentHashMap;
+
+/**
+ * LimelightHelpers provides static methods and classes for interfacing with Limelight vision cameras in FRC.
+ * This library supports all Limelight features including AprilTag tracking, Neural Networks, and standard color/retroreflective tracking.
+ */
+public class LimelightHelpers {
+
+    private static final Map<String, DoubleArrayEntry> doubleArrayEntries = new ConcurrentHashMap<>();
+
+    /**
+     * Represents a Color/Retroreflective Target Result extracted from JSON Output
+     */
+    public static class LimelightTarget_Retro {
+
+        @JsonProperty("t6c_ts")
+        private double[] cameraPose_TargetSpace;
+
+        @JsonProperty("t6r_fs")
+        private double[] robotPose_FieldSpace;
+
+        @JsonProperty("t6r_ts")
+        private  double[] robotPose_TargetSpace;
+
+        @JsonProperty("t6t_cs")
+        private double[] targetPose_CameraSpace;
+
+        @JsonProperty("t6t_rs")
+        private double[] targetPose_RobotSpace;
+
+        public Pose3d getCameraPose_TargetSpace()
+        {
+            return toPose3D(cameraPose_TargetSpace);
+        }
+        public Pose3d getRobotPose_FieldSpace()
+        {
+            return toPose3D(robotPose_FieldSpace);
+        }
+        public Pose3d getRobotPose_TargetSpace()
+        {
+            return toPose3D(robotPose_TargetSpace);
+        }
+        public Pose3d getTargetPose_CameraSpace()
+        {
+            return toPose3D(targetPose_CameraSpace);
+        }
+        public Pose3d getTargetPose_RobotSpace()
+        {
+            return toPose3D(targetPose_RobotSpace);
+        }
+
+        public Pose2d getCameraPose_TargetSpace2D()
+        {
+            return toPose2D(cameraPose_TargetSpace);
+        }
+        public Pose2d getRobotPose_FieldSpace2D()
+        {
+            return toPose2D(robotPose_FieldSpace);
+        }
+        public Pose2d getRobotPose_TargetSpace2D()
+        {
+            return toPose2D(robotPose_TargetSpace);
+        }
+        public Pose2d getTargetPose_CameraSpace2D()
+        {
+            return toPose2D(targetPose_CameraSpace);
+        }
+        public Pose2d getTargetPose_RobotSpace2D()
+        {
+            return toPose2D(targetPose_RobotSpace);
+        }
+
+        @JsonProperty("ta")
+        public double ta;
+
+        @JsonProperty("tx")
+        public double tx;
+        
+        @JsonProperty("ty")
+        public double ty;
+
+        @JsonProperty("txp")
+        public double tx_pixels;
+
+        @JsonProperty("typ")
+        public double ty_pixels;
+
+        @JsonProperty("tx_nocross")
+        public double tx_nocrosshair;
+
+        @JsonProperty("ty_nocross")
+        public double ty_nocrosshair;
+
+        @JsonProperty("ts")
+        public double ts;
+
+        public LimelightTarget_Retro() {
+            cameraPose_TargetSpace = new double[6];
+            robotPose_FieldSpace = new double[6];
+            robotPose_TargetSpace = new double[6];
+            targetPose_CameraSpace = new double[6];
+            targetPose_RobotSpace = new double[6];
+        }
+
+    }
+
+    /**
+     * Represents an AprilTag/Fiducial Target Result extracted from JSON Output
+     */
+    public static class LimelightTarget_Fiducial {
+
+        @JsonProperty("fID")
+        public double fiducialID;
+
+        @JsonProperty("fam")
+        public String fiducialFamily;
+
+        @JsonProperty("t6c_ts")
+        private double[] cameraPose_TargetSpace;
+
+        @JsonProperty("t6r_fs")
+        private double[] robotPose_FieldSpace;
+
+        @JsonProperty("t6r_ts")
+        private double[] robotPose_TargetSpace;
+
+        @JsonProperty("t6t_cs")
+        private double[] targetPose_CameraSpace;
+
+        @JsonProperty("t6t_rs")
+        private double[] targetPose_RobotSpace;
+
+        public Pose3d getCameraPose_TargetSpace()
+        {
+            return toPose3D(cameraPose_TargetSpace);
+        }
+        public Pose3d getRobotPose_FieldSpace()
+        {
+            return toPose3D(robotPose_FieldSpace);
+        }
+        public Pose3d getRobotPose_TargetSpace()
+        {
+            return toPose3D(robotPose_TargetSpace);
+        }
+        public Pose3d getTargetPose_CameraSpace()
+        {
+            return toPose3D(targetPose_CameraSpace);
+        }
+        public Pose3d getTargetPose_RobotSpace()
+        {
+            return toPose3D(targetPose_RobotSpace);
+        }
+
+        public Pose2d getCameraPose_TargetSpace2D()
+        {
+            return toPose2D(cameraPose_TargetSpace);
+        }
+        public Pose2d getRobotPose_FieldSpace2D()
+        {
+            return toPose2D(robotPose_FieldSpace);
+        }
+        public Pose2d getRobotPose_TargetSpace2D()
+        {
+            return toPose2D(robotPose_TargetSpace);
+        }
+        public Pose2d getTargetPose_CameraSpace2D()
+        {
+            return toPose2D(targetPose_CameraSpace);
+        }
+        public Pose2d getTargetPose_RobotSpace2D()
+        {
+            return toPose2D(targetPose_RobotSpace);
+        }
+        
+        @JsonProperty("ta")
+        public double ta;
+
+        @JsonProperty("tx")
+        public double tx;
+
+        @JsonProperty("ty")
+        public double ty;
+
+        @JsonProperty("txp")
+        public double tx_pixels;
+
+        @JsonProperty("typ")
+        public double ty_pixels;
+
+        @JsonProperty("tx_nocross")
+        public double tx_nocrosshair;
+
+        @JsonProperty("ty_nocross")
+        public double ty_nocrosshair;
+
+        @JsonProperty("ts")
+        public double ts;
+        
+        public LimelightTarget_Fiducial() {
+            cameraPose_TargetSpace = new double[6];
+            robotPose_FieldSpace = new double[6];
+            robotPose_TargetSpace = new double[6];
+            targetPose_CameraSpace = new double[6];
+            targetPose_RobotSpace = new double[6];
+        }
+    }
+
+    /**
+     * Represents a Barcode Target Result extracted from JSON Output
+     */
+    public static class LimelightTarget_Barcode {
+
+        /**
+         * Barcode family type (e.g. "QR", "DataMatrix", etc.)
+         */
+        @JsonProperty("fam")
+        public String family;
+
+        /**
+         * Gets the decoded data content of the barcode
+         */
+        @JsonProperty("data") 
+        public String data;
+
+        @JsonProperty("txp")
+        public double tx_pixels;
+
+        @JsonProperty("typ")
+        public double ty_pixels;
+
+        @JsonProperty("tx")
+        public double tx;
+
+        @JsonProperty("ty")
+        public double ty;
+
+        @JsonProperty("tx_nocross")
+        public double tx_nocrosshair;
+
+        @JsonProperty("ty_nocross")
+        public double ty_nocrosshair;
+
+        @JsonProperty("ta")
+        public double ta;
+
+        @JsonProperty("pts")
+        public double[][] corners;
+
+        public LimelightTarget_Barcode() {
+        }
+
+        public String getFamily() {
+            return family;
+        }
+    }
+
+    /**
+     * Represents a Neural Classifier Pipeline Result extracted from JSON Output
+     */
+    public static class LimelightTarget_Classifier {
+
+        @JsonProperty("class")
+        public String className;
+
+        @JsonProperty("classID")
+        public double classID;
+
+        @JsonProperty("conf")
+        public double confidence;
+
+        @JsonProperty("zone")
+        public double zone;
+
+        @JsonProperty("tx")
+        public double tx;
+
+        @JsonProperty("txp")
+        public double tx_pixels;
+
+        @JsonProperty("ty")
+        public double ty;
+
+        @JsonProperty("typ")
+        public double ty_pixels;
+
+        public  LimelightTarget_Classifier() {
+        }
+    }
+
+    /**
+     * Represents a Neural Detector Pipeline Result extracted from JSON Output
+     */
+    public static class LimelightTarget_Detector {
+
+        @JsonProperty("class")
+        public String className;
+
+        @JsonProperty("classID")
+        public double classID;
+
+        @JsonProperty("conf")
+        public double confidence;
+
+        @JsonProperty("ta")
+        public double ta;
+
+        @JsonProperty("tx")
+        public double tx;
+
+        @JsonProperty("ty")
+        public double ty;
+
+        @JsonProperty("txp")
+        public double tx_pixels;
+
+        @JsonProperty("typ")
+        public double ty_pixels;
+
+        @JsonProperty("tx_nocross")
+        public double tx_nocrosshair;
+
+        @JsonProperty("ty_nocross")
+        public double ty_nocrosshair;
+
+        public LimelightTarget_Detector() {
+        }
+    }
+
+    /**
+     * Limelight Results object, parsed from a Limelight's JSON results output.
+     */
+    public static class LimelightResults {
+        
+        public String error;
+        
+        @JsonProperty("pID")
+        public double pipelineID;
+
+        @JsonProperty("tl")
+        public double latency_pipeline;
+
+        @JsonProperty("cl")
+        public double latency_capture;
+
+        public double latency_jsonParse;
+
+        @JsonProperty("ts")
+        public double timestamp_LIMELIGHT_publish;
+
+        @JsonProperty("ts_rio")
+        public double timestamp_RIOFPGA_capture;
+
+        @JsonProperty("v")
+        @JsonFormat(shape = Shape.NUMBER)
+        public boolean valid;
+
+        @JsonProperty("botpose")
+        public double[] botpose;
+
+        @JsonProperty("botpose_wpired")
+        public double[] botpose_wpired;
+
+        @JsonProperty("botpose_wpiblue")
+        public double[] botpose_wpiblue;
+
+        @JsonProperty("botpose_tagcount")
+        public double botpose_tagcount;
+       
+        @JsonProperty("botpose_span")
+        public double botpose_span;
+       
+        @JsonProperty("botpose_avgdist")
+        public double botpose_avgdist;
+       
+        @JsonProperty("botpose_avgarea")
+        public double botpose_avgarea;
+
+        @JsonProperty("t6c_rs")
+        public double[] camerapose_robotspace;
+
+        public Pose3d getBotPose3d() {
+            return toPose3D(botpose);
+        }
+    
+        public Pose3d getBotPose3d_wpiRed() {
+            return toPose3D(botpose_wpired);
+        }
+    
+        public Pose3d getBotPose3d_wpiBlue() {
+            return toPose3D(botpose_wpiblue);
+        }
+
+        public Pose2d getBotPose2d() {
+            return toPose2D(botpose);
+        }
+    
+        public Pose2d getBotPose2d_wpiRed() {
+            return toPose2D(botpose_wpired);
+        }
+    
+        public Pose2d getBotPose2d_wpiBlue() {
+            return toPose2D(botpose_wpiblue);
+        }
+
+        @JsonProperty("Retro")
+        public LimelightTarget_Retro[] targets_Retro;
+
+        @JsonProperty("Fiducial")
+        public LimelightTarget_Fiducial[] targets_Fiducials;
+
+        @JsonProperty("Classifier")
+        public LimelightTarget_Classifier[] targets_Classifier;
+
+        @JsonProperty("Detector")
+        public LimelightTarget_Detector[] targets_Detector;
+
+        @JsonProperty("Barcode")
+        public LimelightTarget_Barcode[] targets_Barcode;
+
+        public LimelightResults() {
+            botpose = new double[6];
+            botpose_wpired = new double[6];
+            botpose_wpiblue = new double[6];
+            camerapose_robotspace = new double[6];
+            targets_Retro = new LimelightTarget_Retro[0];
+            targets_Fiducials = new LimelightTarget_Fiducial[0];
+            targets_Classifier = new LimelightTarget_Classifier[0];
+            targets_Detector = new LimelightTarget_Detector[0];
+            targets_Barcode = new LimelightTarget_Barcode[0];
+
+        }
+
+
+    }
+
+    /**
+     * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output.
+     */
+    public static class RawFiducial {
+        public int id = 0;
+        public double txnc = 0;
+        public double tync = 0;
+        public double ta = 0;
+        public double distToCamera = 0;
+        public double distToRobot = 0;
+        public double ambiguity = 0;
+
+
+        public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) {
+            this.id = id;
+            this.txnc = txnc;
+            this.tync = tync;
+            this.ta = ta;
+            this.distToCamera = distToCamera;
+            this.distToRobot = distToRobot;
+            this.ambiguity = ambiguity;
+        }
+    }
+
+    /**
+     * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output.
+     */
+    public static class RawDetection {
+        public int classId = 0;
+        public double txnc = 0;
+        public double tync = 0;
+        public double ta = 0;
+        public double corner0_X = 0;
+        public double corner0_Y = 0;
+        public double corner1_X = 0;
+        public double corner1_Y = 0;
+        public double corner2_X = 0;
+        public double corner2_Y = 0;
+        public double corner3_X = 0;
+        public double corner3_Y = 0;
+
+
+        public RawDetection(int classId, double txnc, double tync, double ta, 
+            double corner0_X, double corner0_Y, 
+            double corner1_X, double corner1_Y, 
+            double corner2_X, double corner2_Y, 
+            double corner3_X, double corner3_Y ) {
+            this.classId = classId;
+            this.txnc = txnc;
+            this.tync = tync;
+            this.ta = ta;
+            this.corner0_X = corner0_X;
+            this.corner0_Y = corner0_Y;
+            this.corner1_X = corner1_X;
+            this.corner1_Y = corner1_Y;
+            this.corner2_X = corner2_X;
+            this.corner2_Y = corner2_Y;
+            this.corner3_X = corner3_X;
+            this.corner3_Y = corner3_Y;
+        }
+    }
+    
+    /**
+     * Represents a 3D Pose Estimate.
+     */
+    public static class PoseEstimate {
+        public Pose2d pose;
+        public double timestampSeconds;
+        public double latency;
+        public int tagCount;
+        public double tagSpan;
+        public double avgTagDist;
+        public double avgTagArea;
+
+        public RawFiducial[] rawFiducials; 
+        public boolean isMegaTag2;
+
+        /**
+         * Instantiates a PoseEstimate object with default values
+         */
+        public PoseEstimate() {
+            this.pose = new Pose2d();
+            this.timestampSeconds = 0;
+            this.latency = 0;
+            this.tagCount = 0;
+            this.tagSpan = 0;
+            this.avgTagDist = 0;
+            this.avgTagArea = 0;
+            this.rawFiducials = new RawFiducial[]{};
+            this.isMegaTag2 = false;
+        }
+
+        public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, 
+            int tagCount, double tagSpan, double avgTagDist, 
+            double avgTagArea, RawFiducial[] rawFiducials, boolean isMegaTag2) {
+
+            this.pose = pose;
+            this.timestampSeconds = timestampSeconds;
+            this.latency = latency;
+            this.tagCount = tagCount;
+            this.tagSpan = tagSpan;
+            this.avgTagDist = avgTagDist;
+            this.avgTagArea = avgTagArea;
+            this.rawFiducials = rawFiducials;
+            this.isMegaTag2 = isMegaTag2;
+        }
+
+    }
+
+    /**
+     * Encapsulates the state of an internal Limelight IMU.
+     */
+    public static class IMUData {
+        public double robotYaw = 0.0;
+        public double Roll = 0.0;
+        public double Pitch = 0.0;
+        public double Yaw = 0.0;
+        public double gyroX = 0.0;
+        public double gyroY = 0.0;
+        public double gyroZ = 0.0;
+        public double accelX = 0.0;
+        public double accelY = 0.0;
+        public double accelZ = 0.0;
+
+        public IMUData() {}
+
+        public IMUData(double[] imuData) {
+            if (imuData != null && imuData.length >= 10) {
+                this.robotYaw = imuData[0];
+                this.Roll = imuData[1];
+                this.Pitch = imuData[2];
+                this.Yaw = imuData[3];
+                this.gyroX = imuData[4];
+                this.gyroY = imuData[5];
+                this.gyroZ = imuData[6];
+                this.accelX = imuData[7];
+                this.accelY = imuData[8];
+                this.accelZ = imuData[9];
+            }
+        }
+    }
+
+
+    private static ObjectMapper mapper;
+
+    /**
+     * Print JSON Parse time to the console in milliseconds
+     */
+    static boolean profileJSON = false;
+
+    static final String sanitizeName(String name) {
+        if (name == "" || name == null) {
+            return "limelight";
+        }
+        return name;
+    }
+
+    /**
+     * Takes a 6-length array of pose data and converts it to a Pose3d object.
+     * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees.
+     * @param inData Array containing pose data [x, y, z, roll, pitch, yaw]
+     * @return Pose3d object representing the pose, or empty Pose3d if invalid data
+     */
+    public static Pose3d toPose3D(double[] inData){
+        if(inData.length < 6)
+        {
+            //System.err.println("Bad LL 3D Pose Data!");
+            return new Pose3d();
+        }
+        return new Pose3d(
+            new Translation3d(inData[0], inData[1], inData[2]),
+            new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]),
+                    Units.degreesToRadians(inData[5])));
+    }
+
+    /**
+     * Takes a 6-length array of pose data and converts it to a Pose2d object.
+     * Uses only x, y, and yaw components, ignoring z, roll, and pitch.
+     * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees.
+     * @param inData Array containing pose data [x, y, z, roll, pitch, yaw]
+     * @return Pose2d object representing the pose, or empty Pose2d if invalid data
+     */
+    public static Pose2d toPose2D(double[] inData){
+        if(inData.length < 6)
+        {
+            //System.err.println("Bad LL 2D Pose Data!");
+            return new Pose2d();
+        }
+        Translation2d tran2d = new Translation2d(inData[0], inData[1]);
+        Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5]));
+        return new Pose2d(tran2d, r2d);
+    }
+
+    /**
+     * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw].
+     * Translation components are in meters, rotation components are in degrees.
+     * 
+     * @param pose The Pose3d object to convert
+     * @return A 6-element array containing [x, y, z, roll, pitch, yaw]
+     */
+    public static double[] pose3dToArray(Pose3d pose) {
+        double[] result = new double[6];
+        result[0] = pose.getTranslation().getX();
+        result[1] = pose.getTranslation().getY();
+        result[2] = pose.getTranslation().getZ();
+        result[3] = Units.radiansToDegrees(pose.getRotation().getX());
+        result[4] = Units.radiansToDegrees(pose.getRotation().getY());
+        result[5] = Units.radiansToDegrees(pose.getRotation().getZ());
+        return result;
+    }
+
+    /**
+     * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw].
+     * Translation components are in meters, rotation components are in degrees.
+     * Note: z, roll, and pitch will be 0 since Pose2d only contains x, y, and yaw.
+     * 
+     * @param pose The Pose2d object to convert
+     * @return A 6-element array containing [x, y, 0, 0, 0, yaw]
+     */
+    public static double[] pose2dToArray(Pose2d pose) {
+        double[] result = new double[6];
+        result[0] = pose.getTranslation().getX();
+        result[1] = pose.getTranslation().getY();
+        result[2] = 0;
+        result[3] = Units.radiansToDegrees(0);
+        result[4] = Units.radiansToDegrees(0);
+        result[5] = Units.radiansToDegrees(pose.getRotation().getRadians());
+        return result;
+    }
+
+    private static double extractArrayEntry(double[] inData, int position){
+        if(inData.length < position+1)
+        {
+            return 0;
+        }
+        return inData[position];
+    }
+
+    private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName, boolean isMegaTag2) {
+        DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName);
+        
+        TimestampedDoubleArray tsValue = poseEntry.getAtomic();
+        double[] poseArray = tsValue.value;
+        long timestamp = tsValue.timestamp;
+        
+        if (poseArray.length == 0) {
+            // Handle the case where no data is available
+            return null; // or some default PoseEstimate
+        }
+    
+        var pose = toPose2D(poseArray);
+        double latency = extractArrayEntry(poseArray, 6);
+        int tagCount = (int)extractArrayEntry(poseArray, 7);
+        double tagSpan = extractArrayEntry(poseArray, 8);
+        double tagDist = extractArrayEntry(poseArray, 9);
+        double tagArea = extractArrayEntry(poseArray, 10);
+        
+        // Convert server timestamp from microseconds to seconds and adjust for latency
+        double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0);
+    
+        RawFiducial[] rawFiducials = new RawFiducial[tagCount];
+        int valsPerFiducial = 7;
+        int expectedTotalVals = 11 + valsPerFiducial * tagCount;
+    
+        if (poseArray.length != expectedTotalVals) {
+            // Don't populate fiducials
+        } else {
+            for(int i = 0; i < tagCount; i++) {
+                int baseIndex = 11 + (i * valsPerFiducial);
+                int id = (int)poseArray[baseIndex];
+                double txnc = poseArray[baseIndex + 1];
+                double tync = poseArray[baseIndex + 2];
+                double ta = poseArray[baseIndex + 3];
+                double distToCamera = poseArray[baseIndex + 4];
+                double distToRobot = poseArray[baseIndex + 5];
+                double ambiguity = poseArray[baseIndex + 6];
+                rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity);
+            }
+        }
+    
+        return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials, isMegaTag2);
+    }
+
+    /**
+     * Gets the latest raw fiducial/AprilTag detection results from NetworkTables.
+     * 
+     * @param limelightName Name/identifier of the Limelight
+     * @return Array of RawFiducial objects containing detection details
+     */
+    public static RawFiducial[] getRawFiducials(String limelightName) {
+        var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials");
+        var rawFiducialArray = entry.getDoubleArray(new double[0]);
+        int valsPerEntry = 7;
+        if (rawFiducialArray.length % valsPerEntry != 0) {
+            return new RawFiducial[0];
+        }
+    
+        int numFiducials = rawFiducialArray.length / valsPerEntry;
+        RawFiducial[] rawFiducials = new RawFiducial[numFiducials];
+    
+        for (int i = 0; i < numFiducials; i++) {
+            int baseIndex = i * valsPerEntry;
+            int id = (int) extractArrayEntry(rawFiducialArray, baseIndex);
+            double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1);
+            double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2);
+            double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3);
+            double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4);
+            double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5);
+            double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6);
+            
+            rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity);
+        }
+    
+        return rawFiducials;
+    }
+
+    /**
+     * Gets the latest raw neural detector results from NetworkTables
+     *
+     * @param limelightName Name/identifier of the Limelight
+     * @return Array of RawDetection objects containing detection details
+     */
+    public static RawDetection[] getRawDetections(String limelightName) {
+        var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections");
+        var rawDetectionArray = entry.getDoubleArray(new double[0]);
+        int valsPerEntry = 12;
+        if (rawDetectionArray.length % valsPerEntry != 0) {
+            return new RawDetection[0];
+        }
+    
+        int numDetections = rawDetectionArray.length / valsPerEntry;
+        RawDetection[] rawDetections = new RawDetection[numDetections];
+    
+        for (int i = 0; i < numDetections; i++) {
+            int baseIndex = i * valsPerEntry; // Starting index for this detection's data
+            int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex);
+            double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1);
+            double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2);
+            double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3);
+            double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4);
+            double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5);
+            double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6);
+            double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7);
+            double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8);
+            double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9);
+            double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10);
+            double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11);
+            
+            rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y);
+        }
+    
+        return rawDetections;
+    }
+
+    /**
+     * Prints detailed information about a PoseEstimate to standard output.
+     * Includes timestamp, latency, tag count, tag span, average tag distance,
+     * average tag area, and detailed information about each detected fiducial.
+     *
+     * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available."
+     */
+    public static void printPoseEstimate(PoseEstimate pose) {
+        if (pose == null) {
+            System.out.println("No PoseEstimate available.");
+            return;
+        }
+    
+        System.out.printf("Pose Estimate Information:%n");
+        System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds);
+        System.out.printf("Latency: %.3f ms%n", pose.latency);
+        System.out.printf("Tag Count: %d%n", pose.tagCount);
+        System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan);
+        System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist);
+        System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea);
+        System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2);
+        System.out.println();
+    
+        if (pose.rawFiducials == null || pose.rawFiducials.length == 0) {
+            System.out.println("No RawFiducials data available.");
+            return;
+        }
+    
+        System.out.println("Raw Fiducials Details:");
+        for (int i = 0; i < pose.rawFiducials.length; i++) {
+            RawFiducial fiducial = pose.rawFiducials[i];
+            System.out.printf(" Fiducial #%d:%n", i + 1);
+            System.out.printf("  ID: %d%n", fiducial.id);
+            System.out.printf("  TXNC: %.2f%n", fiducial.txnc);
+            System.out.printf("  TYNC: %.2f%n", fiducial.tync);
+            System.out.printf("  TA: %.2f%n", fiducial.ta);
+            System.out.printf("  Distance to Camera: %.2f meters%n", fiducial.distToCamera);
+            System.out.printf("  Distance to Robot: %.2f meters%n", fiducial.distToRobot);
+            System.out.printf("  Ambiguity: %.2f%n", fiducial.ambiguity);
+            System.out.println();
+        }
+    }
+
+    public static Boolean validPoseEstimate(PoseEstimate pose) {
+        return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0;
+    }
+
+    public static NetworkTable getLimelightNTTable(String tableName) {
+        return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName));
+    }
+
+    public static void Flush() {
+        NetworkTableInstance.getDefault().flush();
+    }
+
+    public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) {
+        return getLimelightNTTable(tableName).getEntry(entryName);
+    }
+
+    public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) {
+        String key = tableName + "/" + entryName;
+        return doubleArrayEntries.computeIfAbsent(key, k -> {
+            NetworkTable table = getLimelightNTTable(tableName);
+            return table.getDoubleArrayTopic(entryName).getEntry(new double[0]);
+        });
+    }
+    
+    public static double getLimelightNTDouble(String tableName, String entryName) {
+        return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0);
+    }
+
+    public static void setLimelightNTDouble(String tableName, String entryName, double val) {
+        getLimelightNTTableEntry(tableName, entryName).setDouble(val);
+    }
+
+    public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) {
+        getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val);
+    }
+
+    public static double[] getLimelightNTDoubleArray(String tableName, String entryName) {
+        return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]);
+    }
+
+
+    public static String getLimelightNTString(String tableName, String entryName) {
+        return getLimelightNTTableEntry(tableName, entryName).getString("");
+    }
+
+    public static String[] getLimelightNTStringArray(String tableName, String entryName) {
+        return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]);
+    }
+
+
+    public static URL getLimelightURLString(String tableName, String request) {
+        String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request;
+        URL url;
+        try {
+            url = new URL(urlString);
+            return url;
+        } catch (MalformedURLException e) {
+            System.err.println("bad LL URL");
+        }
+        return null;
+    }
+    /////
+    /////
+
+    /**
+     * Does the Limelight have a valid target?
+     * @param limelightName Name of the Limelight camera ("" for default)
+     * @return True if a valid target is present, false otherwise
+     */
+    public static boolean getTV(String limelightName) {
+        return 1.0 == getLimelightNTDouble(limelightName, "tv");
+    }
+
+    /**
+     * Gets the horizontal offset from the crosshair to the target in degrees.
+     * @param limelightName Name of the Limelight camera ("" for default)
+     * @return Horizontal offset angle in degrees
+     */
+    public static double getTX(String limelightName) {
+        return getLimelightNTDouble(limelightName, "tx");
+    }
+
+    /**
+     * Gets the vertical offset from the crosshair to the target in degrees.
+     * @param limelightName Name of the Limelight camera ("" for default)
+     * @return Vertical offset angle in degrees
+     */
+    public static double getTY(String limelightName) {
+        return getLimelightNTDouble(limelightName, "ty");
+    }
+
+    /**
+     * Gets the horizontal offset from the principal pixel/point to the target in degrees.  This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality.
+     * @param limelightName Name of the Limelight camera ("" for default)
+     * @return Horizontal offset angle in degrees
+     */
+    public static double getTXNC(String limelightName) {
+        return getLimelightNTDouble(limelightName, "txnc");
+    }
+
+    /**
+     * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality.
+     * @param limelightName Name of the Limelight camera ("" for default)
+     * @return Vertical offset angle in degrees
+     */
+    public static double getTYNC(String limelightName) {
+        return getLimelightNTDouble(limelightName, "tync");
+    }
+
+    /**
+     * Gets the target area as a percentage of the image (0-100%).
+     * @param limelightName Name of the Limelight camera ("" for default) 
+     * @return Target area percentage (0-100)
+     */
+    public static double getTA(String limelightName) {
+        return getLimelightNTDouble(limelightName, "ta");
+    }
+
+    /**
+     * T2D is an array that contains several targeting metrcis
+     * @param limelightName Name of the Limelight camera
+     * @return Array containing  [targetValid, targetCount, targetLatency, captureLatency, tx, ty, txnc, tync, ta, tid, targetClassIndexDetector, 
+     * targetClassIndexClassifier, targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, targetVerticalExtentPixels, targetSkewDegrees]
+     */
+    public static double[] getT2DArray(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "t2d");
+    }
+    
+    /**
+     * Gets the number of targets currently detected.
+     * @param limelightName Name of the Limelight camera
+     * @return Number of detected targets
+     */
+    public static int getTargetCount(String limelightName) {
+      double[] t2d = getT2DArray(limelightName);
+      if(t2d.length == 17)
+      {
+        return (int)t2d[1];
+      }
+      return 0;
+    }
+
+    /**
+     * Gets the classifier class index from the currently running neural classifier pipeline
+     * @param limelightName Name of the Limelight camera
+     * @return Class index from classifier pipeline
+     */
+    public static int getClassifierClassIndex (String limelightName) {
+    double[] t2d = getT2DArray(limelightName);
+      if(t2d.length == 17)
+      {
+        return (int)t2d[10];
+      }
+      return 0;
+    }
+
+    /**
+     * Gets the detector class index from the primary result of the currently running neural detector pipeline.
+     * @param limelightName Name of the Limelight camera
+     * @return Class index from detector pipeline
+     */
+    public static int getDetectorClassIndex (String limelightName) {
+     double[] t2d = getT2DArray(limelightName);
+      if(t2d.length == 17)
+      {
+        return (int)t2d[11];
+      }
+      return 0;
+    }
+
+    /**
+     * Gets the current neural classifier result class name.
+     * @param limelightName Name of the Limelight camera
+     * @return Class name string from classifier pipeline
+     */
+    public static String getClassifierClass (String limelightName) {
+        return getLimelightNTString(limelightName, "tcclass");
+    }
+
+    /**
+     * Gets the primary neural detector result class name.
+     * @param limelightName Name of the Limelight camera
+     * @return Class name string from detector pipeline
+     */
+    public static String getDetectorClass (String limelightName) {
+        return getLimelightNTString(limelightName, "tdclass");
+    }
+
+    /**
+     * Gets the pipeline's processing latency contribution.
+     * @param limelightName Name of the Limelight camera
+     * @return Pipeline latency in milliseconds
+     */
+    public static double getLatency_Pipeline(String limelightName) {
+        return getLimelightNTDouble(limelightName, "tl");
+    }
+
+    /**
+     * Gets the capture latency.
+     * @param limelightName Name of the Limelight camera
+     * @return Capture latency in milliseconds
+     */
+    public static double getLatency_Capture(String limelightName) {
+        return getLimelightNTDouble(limelightName, "cl");
+    }
+
+    /**
+     * Gets the active pipeline index.
+     * @param limelightName Name of the Limelight camera
+     * @return Current pipeline index (0-9)
+     */
+    public static double getCurrentPipelineIndex(String limelightName) {
+        return getLimelightNTDouble(limelightName, "getpipe");
+    }
+
+    /**
+     * Gets the current pipeline type.
+     * @param limelightName Name of the Limelight camera
+     * @return Pipeline type string (e.g. "retro", "apriltag", etc)
+     */
+    public static String getCurrentPipelineType(String limelightName) {
+        return getLimelightNTString(limelightName, "getpipetype");
+    }
+
+    /**
+     * Gets the full JSON results dump.
+     * @param limelightName Name of the Limelight camera
+     * @return JSON string containing all current results
+     */
+    public static String getJSONDump(String limelightName) {
+        return getLimelightNTString(limelightName, "json");
+    }
+
+    /**
+     * Switch to getBotPose
+     * 
+     * @param limelightName
+     * @return
+     */
+    @Deprecated
+    public static double[] getBotpose(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "botpose");
+    }
+
+    /**
+     * Switch to getBotPose_wpiRed
+     * 
+     * @param limelightName
+     * @return
+     */
+    @Deprecated
+    public static double[] getBotpose_wpiRed(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "botpose_wpired");
+    }
+
+    /**
+     * Switch to getBotPose_wpiBlue
+     * 
+     * @param limelightName
+     * @return
+     */
+    @Deprecated
+    public static double[] getBotpose_wpiBlue(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue");
+    }
+
+    public static double[] getBotPose(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "botpose");
+    }
+
+    public static double[] getBotPose_wpiRed(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "botpose_wpired");
+    }
+
+    public static double[] getBotPose_wpiBlue(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue");
+    }
+
+    public static double[] getBotPose_TargetSpace(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "botpose_targetspace");
+    }
+
+    public static double[] getCameraPose_TargetSpace(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace");
+    }
+
+    public static double[] getTargetPose_CameraSpace(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace");
+    }
+
+    public static double[] getTargetPose_RobotSpace(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace");
+    }
+
+    public static double[] getTargetColor(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "tc");
+    }
+
+    public static double getFiducialID(String limelightName) {
+        return getLimelightNTDouble(limelightName, "tid");
+    }
+
+    public static String getNeuralClassID(String limelightName) {
+        return getLimelightNTString(limelightName, "tclass");
+    }
+
+    public static String[] getRawBarcodeData(String limelightName) {
+        return getLimelightNTStringArray(limelightName, "rawbarcodes");
+    }
+
+    /////
+    /////
+
+    public static Pose3d getBotPose3d(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose");
+        return toPose3D(poseArray);
+    }
+
+    /**
+     * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System.
+     * @param limelightName Name/identifier of the Limelight
+     * @return Pose3d object representing the robot's position and orientation in Red Alliance field space
+     */
+    public static Pose3d getBotPose3d_wpiRed(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired");
+        return toPose3D(poseArray);
+    }
+
+    /**
+     * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System.
+     * @param limelightName Name/identifier of the Limelight
+     * @return Pose3d object representing the robot's position and orientation in Blue Alliance field space
+     */
+    public static Pose3d getBotPose3d_wpiBlue(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue");
+        return toPose3D(poseArray);
+    }
+
+    /**
+     * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system.
+     * @param limelightName Name/identifier of the Limelight
+     * @return Pose3d object representing the robot's position and orientation relative to the target
+     */
+    public static Pose3d getBotPose3d_TargetSpace(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace");
+        return toPose3D(poseArray);
+    }
+
+    /**
+     * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system.
+     * @param limelightName Name/identifier of the Limelight
+     * @return Pose3d object representing the camera's position and orientation relative to the target
+     */
+    public static Pose3d getCameraPose3d_TargetSpace(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace");
+        return toPose3D(poseArray);
+    }
+
+    /**
+     * Gets the target's 3D pose with respect to the camera's coordinate system.
+     * @param limelightName Name/identifier of the Limelight
+     * @return Pose3d object representing the target's position and orientation relative to the camera
+     */
+    public static Pose3d getTargetPose3d_CameraSpace(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace");
+        return toPose3D(poseArray);
+    }
+
+    /**
+     * Gets the target's 3D pose with respect to the robot's coordinate system.
+     * @param limelightName Name/identifier of the Limelight
+     * @return Pose3d object representing the target's position and orientation relative to the robot
+     */
+    public static Pose3d getTargetPose3d_RobotSpace(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace");
+        return toPose3D(poseArray);
+    }
+
+    /**
+     * Gets the camera's 3D pose with respect to the robot's coordinate system.
+     * @param limelightName Name/identifier of the Limelight
+     * @return Pose3d object representing the camera's position and orientation relative to the robot
+     */
+    public static Pose3d getCameraPose3d_RobotSpace(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace");
+        return toPose3D(poseArray);
+    }
+
+    /**
+     * Gets the Pose2d for easy use with Odometry vision pose estimator
+     * (addVisionMeasurement)
+     * 
+     * @param limelightName
+     * @return
+     */
+    public static Pose2d getBotPose2d_wpiBlue(String limelightName) {
+
+        double[] result = getBotPose_wpiBlue(limelightName);
+        return toPose2D(result);
+    }
+
+    /**
+     * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system.
+     * 
+     * @param limelightName
+     * @return
+     */
+    public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) {
+        return getBotPoseEstimate(limelightName, "botpose_wpiblue", false);
+    }
+
+    /**
+     * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system.
+     * Make sure you are calling setRobotOrientation() before calling this method.
+     * 
+     * @param limelightName
+     * @return
+     */
+    public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) {
+        return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true);
+    }
+
+    /**
+     * Gets the Pose2d for easy use with Odometry vision pose estimator
+     * (addVisionMeasurement)
+     * 
+     * @param limelightName
+     * @return
+     */
+    public static Pose2d getBotPose2d_wpiRed(String limelightName) {
+
+        double[] result = getBotPose_wpiRed(limelightName);
+        return toPose2D(result);
+
+    }
+
+    /**
+     * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED
+     * alliance
+     * @param limelightName
+     * @return
+     */
+    public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) {
+        return getBotPoseEstimate(limelightName, "botpose_wpired", false);
+    }
+
+    /**
+     * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED
+     * alliance
+     * @param limelightName
+     * @return
+     */
+    public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) {
+        return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true);
+    }
+
+    /**
+     * Gets the Pose2d for easy use with Odometry vision pose estimator
+     * (addVisionMeasurement)
+     * 
+     * @param limelightName
+     * @return
+     */
+    public static Pose2d getBotPose2d(String limelightName) {
+
+        double[] result = getBotPose(limelightName);
+        return toPose2D(result);
+
+    }
+   
+    /**
+     * Gets the current IMU data from NetworkTables.
+     * IMU data is formatted as [robotYaw, Roll, Pitch, Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ].
+     * Returns all zeros if data is invalid or unavailable.
+     * 
+     * @param limelightName Name/identifier of the Limelight
+     * @return IMUData object containing all current IMU data
+     */
+    public static IMUData getIMUData(String limelightName) {
+        double[] imuData = getLimelightNTDoubleArray(limelightName, "imu");
+        if (imuData == null || imuData.length < 10) {
+            return new IMUData();  // Returns object with all zeros
+        }
+        return new IMUData(imuData);
+    }
+
+    /////
+    /////
+
+    public static void setPipelineIndex(String limelightName, int pipelineIndex) {
+        setLimelightNTDouble(limelightName, "pipeline", pipelineIndex);
+    }
+
+    
+    public static void setPriorityTagID(String limelightName, int ID) {
+        setLimelightNTDouble(limelightName, "priorityid", ID);
+    }
+
+    /**
+     * Sets LED mode to be controlled by the current pipeline.
+     * @param limelightName Name of the Limelight camera
+     */
+    public static void setLEDMode_PipelineControl(String limelightName) {
+        setLimelightNTDouble(limelightName, "ledMode", 0);
+    }
+
+    public static void setLEDMode_ForceOff(String limelightName) {
+        setLimelightNTDouble(limelightName, "ledMode", 1);
+    }
+
+    public static void setLEDMode_ForceBlink(String limelightName) {
+        setLimelightNTDouble(limelightName, "ledMode", 2);
+    }
+
+    public static void setLEDMode_ForceOn(String limelightName) {
+        setLimelightNTDouble(limelightName, "ledMode", 3);
+    }
+
+    /**
+     * Enables standard side-by-side stream mode.
+     * @param limelightName Name of the Limelight camera
+     */
+    public static void setStreamMode_Standard(String limelightName) {
+        setLimelightNTDouble(limelightName, "stream", 0);
+    }
+
+    /**
+     * Enables Picture-in-Picture mode with secondary stream in the corner.
+     * @param limelightName Name of the Limelight camera
+     */
+    public static void setStreamMode_PiPMain(String limelightName) {
+        setLimelightNTDouble(limelightName, "stream", 1);
+    }
+
+    /**
+     * Enables Picture-in-Picture mode with primary stream in the corner.
+     * @param limelightName Name of the Limelight camera
+     */
+    public static void setStreamMode_PiPSecondary(String limelightName) {
+        setLimelightNTDouble(limelightName, "stream", 2);
+    }
+
+
+    /**
+     * Sets the crop window for the camera. The crop window in the UI must be completely open.
+     * @param limelightName Name of the Limelight camera
+     * @param cropXMin Minimum X value (-1 to 1)
+     * @param cropXMax Maximum X value (-1 to 1)
+     * @param cropYMin Minimum Y value (-1 to 1)
+     * @param cropYMax Maximum Y value (-1 to 1)
+     */
+    public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) {
+        double[] entries = new double[4];
+        entries[0] = cropXMin;
+        entries[1] = cropXMax;
+        entries[2] = cropYMin;
+        entries[3] = cropYMax;
+        setLimelightNTDoubleArray(limelightName, "crop", entries);
+    }
+   
+    /**
+     * Sets 3D offset point for easy 3D targeting.
+     */
+    public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) {
+        double[] entries = new double[3];
+        entries[0] = offsetX;
+        entries[1] = offsetY;
+        entries[2] = offsetZ;
+        setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries);
+    }
+
+    /**
+     * Sets robot orientation values used by MegaTag2 localization algorithm.
+     * 
+     * @param limelightName Name/identifier of the Limelight
+     * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC
+     * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second
+     * @param pitch (Unnecessary) Robot pitch in degrees 
+     * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second
+     * @param roll (Unnecessary) Robot roll in degrees
+     * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second
+     */
+    public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, 
+        double pitch, double pitchRate, 
+        double roll, double rollRate) {
+        SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true);
+    }
+
+    public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, 
+        double pitch, double pitchRate, 
+        double roll, double rollRate) {
+        SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false);
+    }
+
+    private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, 
+        double pitch, double pitchRate, 
+        double roll, double rollRate, boolean flush) {
+
+        double[] entries = new double[6];
+        entries[0] = yaw;
+        entries[1] = yawRate;
+        entries[2] = pitch;
+        entries[3] = pitchRate;
+        entries[4] = roll;
+        entries[5] = rollRate;
+        setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries);
+        if(flush)
+        {
+            Flush();
+        }
+    }
+   
+    /**
+     * Configures the IMU mode for MegaTag2 Localization
+     * 
+     * @param limelightName Name/identifier of the Limelight
+     * @param mode IMU mode.
+     */
+    public static void SetIMUMode(String limelightName, int mode) {
+        setLimelightNTDouble(limelightName, "imumode_set", mode);
+    }
+
+    /**
+     * Sets the 3D point-of-interest offset for the current fiducial pipeline. 
+     * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking
+     *
+     * @param limelightName Name/identifier of the Limelight
+     * @param x X offset in meters
+     * @param y Y offset in meters
+     * @param z Z offset in meters
+     */
+    public static void SetFidcuial3DOffset(String limelightName, double x, double y, 
+        double z) {
+
+        double[] entries = new double[3];
+        entries[0] = x;
+        entries[1] = y;
+        entries[2] = z;
+        setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries);
+    }
+
+    /**
+     * Overrides the valid AprilTag IDs that will be used for localization.
+     * Tags not in this list will be ignored for robot pose estimation.
+     *
+     * @param limelightName Name/identifier of the Limelight
+     * @param validIDs Array of valid AprilTag IDs to track
+     */
+    public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) {
+        double[] validIDsDouble = new double[validIDs.length];
+        for (int i = 0; i < validIDs.length; i++) {
+            validIDsDouble[i] = validIDs[i];
+        }        
+        setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble);
+    }
+
+    /**
+     * Sets the downscaling factor for AprilTag detection.
+     * Increasing downscale can improve performance at the cost of potentially reduced detection range.
+     * 
+     * @param limelightName Name/identifier of the Limelight
+     * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to 0 for pipeline control.
+     */
+    public static void SetFiducialDownscalingOverride(String limelightName, float downscale) 
+    {
+        int d = 0; // pipeline
+        if (downscale == 1.0)
+        {
+            d = 1;
+        }
+        if (downscale == 1.5)
+        {
+            d = 2;
+        }
+        if (downscale == 2)
+        {
+            d = 3;
+        }
+        if (downscale == 3)
+        {
+            d = 4;
+        }
+        if (downscale == 4)
+        {
+            d = 5;
+        }
+        setLimelightNTDouble(limelightName, "fiducial_downscale_set", d);
+    }
+    
+    /**
+     * Sets the camera pose relative to the robot.
+     * @param limelightName Name of the Limelight camera
+     * @param forward Forward offset in meters
+     * @param side Side offset in meters
+     * @param up Up offset in meters
+     * @param roll Roll angle in degrees
+     * @param pitch Pitch angle in degrees
+     * @param yaw Yaw angle in degrees
+     */
+    public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) {
+        double[] entries = new double[6];
+        entries[0] = forward;
+        entries[1] = side;
+        entries[2] = up;
+        entries[3] = roll;
+        entries[4] = pitch;
+        entries[5] = yaw;
+        setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries);
+    }
+
+    /////
+    /////
+
+    public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) {
+        setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData);
+    }
+
+    public static double[] getPythonScriptData(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "llpython");
+    }
+
+    /////
+    /////
+
+    /**
+     * Asynchronously take snapshot.
+     */
+    public static CompletableFuture<Boolean> takeSnapshot(String tableName, String snapshotName) {
+        return CompletableFuture.supplyAsync(() -> {
+            return SYNCH_TAKESNAPSHOT(tableName, snapshotName);
+        });
+    }
+
+    private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) {
+        URL url = getLimelightURLString(tableName, "capturesnapshot");
+        try {
+            HttpURLConnection connection = (HttpURLConnection) url.openConnection();
+            connection.setRequestMethod("GET");
+            if (snapshotName != null && snapshotName != "") {
+                connection.setRequestProperty("snapname", snapshotName);
+            }
+
+            int responseCode = connection.getResponseCode();
+            if (responseCode == 200) {
+                return true;
+            } else {
+                System.err.println("Bad LL Request");
+            }
+        } catch (IOException e) {
+            System.err.println(e.getMessage());
+        }
+        return false;
+    }
+
+    /**
+     * Gets the latest JSON results output and returns a LimelightResults object.
+     * @param limelightName Name of the Limelight camera
+     * @return LimelightResults object containing all current target data
+     */
+    public static LimelightResults getLatestResults(String limelightName) {
+
+        long start = System.nanoTime();
+        LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults();
+        if (mapper == null) {
+            mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false);
+        }
+
+        try {
+            results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class);
+        } catch (JsonProcessingException e) {
+            results.error = "lljson error: " + e.getMessage();
+        }
+
+        long end = System.nanoTime();
+        double millis = (end - start) * .000001;
+        results.latency_jsonParse = millis;
+        if (profileJSON) {
+            System.out.printf("lljson: %.2f\r\n", millis);
+        }
+
+        return results;
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index a33249e..4679afa 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,13 +4,25 @@
 
 package frc.robot;
 
-import frc.robot.Constants.OperatorConstants;
-import frc.robot.commands.Autos;
-import frc.robot.commands.ExampleCommand;
-import frc.robot.subsystems.ExampleSubsystem;
+import java.io.File;
+import java.util.Optional;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.studica.frc.AHRS;
+import com.studica.frc.AHRS.NavXComType;
+
+import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.Constants.DrivetrainConstants;
+import frc.robot.Constants.OperatorConstants;
+import frc.robot.factorys.DriveSubsystemFactory;
+import frc.robot.factorys.TalonFXFactory;
+import frc.robot.subsystems.DriveSubsystem;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -20,18 +32,83 @@
  */
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
-  private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
+  private final Optional<DriveSubsystem> m_driveSubsystem;
 
   // Replace with CommandPS4Controller or CommandJoystick if needed
   private final CommandXboxController m_driverController =
       new CommandXboxController(OperatorConstants.kDriverControllerPort);
 
+  private final AHRS m_gyro = new AHRS(NavXComType.kMXP_SPI);
+  private final DifferentialDriveKinematics m_DriveKinematics = new DifferentialDriveKinematics(DrivetrainConstants.trackWidthMeters);
+
+  // Pose Estimators
+  private DifferentialDrivePoseEstimator m_DrivePoseEstimator = new DifferentialDrivePoseEstimator(
+                                                                  m_DriveKinematics, 
+                                                                  m_gyro.getRotation2d(), 
+                                                                  0, 
+                                                                  0, 
+                                                                  new Pose2d());
+
+  // Factorys
+  private TalonFXFactory m_TalonFXFactory = new TalonFXFactory();
+  private DriveSubsystemFactory m_DriveSubsystemFactory = new DriveSubsystemFactory();
+
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
+    // Init DriveSubsystem
+    Optional<TalonFX> rightLead = m_TalonFXFactory.construct(DrivetrainConstants.kRightMotorCANID);
+    Optional<TalonFX> leftLead = m_TalonFXFactory.construct(DrivetrainConstants.kLeftMotorCANID);
+    Optional<TalonFX> rightFollower = m_TalonFXFactory.construct(DrivetrainConstants.kOptionalRightMotorCANID);
+    Optional<TalonFX> leftFollower = m_TalonFXFactory.construct(DrivetrainConstants.kOptionalLeftMotorCANID);
+    m_driveSubsystem = m_DriveSubsystemFactory.construct(m_DrivePoseEstimator, m_DriveKinematics, m_gyro, rightLead, leftLead, rightFollower, leftFollower);
+
+    // Init the subsystems
+    //m_limelightSubsystem = getSubsystem(LimelightSubsystem.class, m_limeLightPoseEstimator);
+    //m_exampleSubsystem = getSubsystem(ExampleSubsystem.class);
+
+    // Init Autos (/home/lvuser/deploy/pathplanner/autos)
+    //File directory = new File("/home/lvuser/deploy/pathplanner/autos");
+
+    // Init Chooser
+    // autoChooser = AutoBuilder.buildAutoChooser();
+
+    // Configure the default commands
+    configureDefaultCommands();
+
     // Configure the trigger bindings
     configureBindings();
   }
 
+  // Tom wrote this cool template to make the optional subsystem creation code in
+  // the constructor above a lot clearer. This is what clever coding looks like.
+  // Owen: I added the ability to pass object args through this for dependency injection,
+  // I hope this does not break anything or commited a coding sin or somthing
+  // private static <SSC> Optional<SSC> getSubsystem(Class<SSC> subsystemClass, Object... args) {
+  //   Optional<SSC> iss;
+  //   try {
+
+  //     iss = Optional.ofNullable(subsystemClass.getDeclaredConstructor().newInstance(args));
+  //   } catch (Exception e) {
+  //     iss = Optional.empty();
+  //     // This is not tested! - Owen
+  //     DriverStation.reportWarning(
+  //       String.format(
+  //       "The %s was not found!", subsystemClass.getName()), 
+  //       false
+  //     );
+  //   }
+  //   return iss;
+  // }
+
+  private void configureDefaultCommands() {
+    if (m_driveSubsystem.isPresent())
+    {
+      DriveSubsystem driveSubsystem = m_driveSubsystem.get();
+
+      driveSubsystem.initDefaultCommand(m_driverController);
+    }
+  }
+
   /**
    * Use this method to define your trigger->command mappings. Triggers can be created via the
    * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary
@@ -42,13 +119,20 @@ public RobotContainer() {
    * joysticks}.
    */
   private void configureBindings() {
-    // Schedule `ExampleCommand` when `exampleCondition` changes to `true`
-    new Trigger(m_exampleSubsystem::exampleCondition)
-        .onTrue(new ExampleCommand(m_exampleSubsystem));
+    // SmartDashboard.putBoolean("Example Subsystem", m_exampleSubsystem.isPresent());
+
+    // if (m_exampleSubsystem.isPresent())
+    // {
+    //   ExampleSubsystem exampleSubsystem = m_exampleSubsystem.get();
+
+    //   // Schedule `ExampleCommand` when `exampleCondition` changes to `true`
+    //   new Trigger(exampleSubsystem::exampleCondition)
+    //       .onTrue(new ExampleCommand(exampleSubsystem));
 
-    // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed,
-    // cancelling on release.
-    m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
+    //   // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed,
+    //   // cancelling on release.
+    //   m_driverController.b().whileTrue(exampleSubsystem.exampleMethodCommand());
+    // }
   }
 
   /**
@@ -58,6 +142,6 @@ private void configureBindings() {
    */
   public Command getAutonomousCommand() {
     // An example command will be run in autonomous
-    return Autos.exampleAuto(m_exampleSubsystem);
+    return null; //Autos.exampleAuto(exampleSubsystem);
   }
 }
diff --git a/src/main/java/frc/robot/commands/ArcadeDriveCommand.java b/src/main/java/frc/robot/commands/ArcadeDriveCommand.java
new file mode 100644
index 0000000..aa59995
--- /dev/null
+++ b/src/main/java/frc/robot/commands/ArcadeDriveCommand.java
@@ -0,0 +1,53 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+import frc.robot.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+
+/** An example command that uses an example subsystem. */
+public class ArcadeDriveCommand extends Command {
+  private final DriveSubsystem m_subsystem;
+  private CommandXboxController m_driveController;
+
+  /**
+   * Creates a new ArcadeDriveCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public ArcadeDriveCommand(DriveSubsystem subsystem, CommandXboxController driveController)  {
+    m_subsystem = subsystem;
+    m_driveController = driveController;
+
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(m_subsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    // Note: We negate both axis values so that pushing the joystick forwards
+    // (which makes the readin more negative) increases the speed and twisting clockwise
+    // turns the robot clockwise.
+    m_subsystem.setArcadeSpeeds(
+      -m_driveController.getRightY(), 
+      -m_driveController.getLeftX()
+    );
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/factorys/DriveSubsystemFactory.java b/src/main/java/frc/robot/factorys/DriveSubsystemFactory.java
new file mode 100644
index 0000000..afc5b84
--- /dev/null
+++ b/src/main/java/frc/robot/factorys/DriveSubsystemFactory.java
@@ -0,0 +1,33 @@
+package frc.robot.factorys;
+
+import java.util.Optional;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.studica.frc.AHRS;
+
+import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import frc.robot.subsystems.DriveSubsystem;
+
+public class DriveSubsystemFactory {
+    public DriveSubsystemFactory() {
+    }
+
+    public Optional<DriveSubsystem> construct(DifferentialDrivePoseEstimator poseEstimator,
+            DifferentialDriveKinematics kinematics,
+            AHRS gyro, Optional<TalonFX> rightLead, Optional<TalonFX> leftLead, Optional<TalonFX> rightFollower,
+            Optional<TalonFX> leftFollower) {
+
+        if (rightLead.isEmpty() || leftLead.isEmpty()) {
+            return Optional.empty();
+        }
+
+        DriveSubsystem driveSubsystem = new DriveSubsystem(poseEstimator, kinematics, gyro, rightLead.get(),
+                leftLead.get());
+
+        if (rightFollower.isPresent() && leftFollower.isPresent()) {
+            driveSubsystem.setFollowers(rightFollower.get(), leftFollower.get());
+        }
+
+        return Optional.of(driveSubsystem);
+    }
+}
diff --git a/src/main/java/frc/robot/factorys/TalonFXFactory.java b/src/main/java/frc/robot/factorys/TalonFXFactory.java
new file mode 100644
index 0000000..1a84899
--- /dev/null
+++ b/src/main/java/frc/robot/factorys/TalonFXFactory.java
@@ -0,0 +1,20 @@
+package frc.robot.factorys;
+
+import java.util.Optional;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+
+public class TalonFXFactory {
+    public TalonFXFactory() {}
+
+    public Optional<TalonFX> construct(int CANID) {
+        Optional<TalonFX> newMotor;
+        try {
+            newMotor = Optional.of(new TalonFX(CANID));
+        } catch (Exception e) {
+            newMotor = Optional.empty();
+        }
+
+        return newMotor;
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
index 01d29cb..fe79d2b 100644
--- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
@@ -3,45 +3,190 @@
 // the WPILib BSD license file in the root directory of this project.
 
 package frc.robot.subsystems;
+import static edu.wpi.first.units.Units.*;
 
-import com.ctre.phoenix6.configs.MotorOutputConfigs;
-import com.ctre.phoenix6.configs.TalonFXConfigurator;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.studica.frc.AHRS;
+import com.studica.frc.AHRS.NavXComType;
 
+import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Constants.DrivetrainConstants;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.Constants.*;
+import frc.robot.commands.ArcadeDriveCommand;
 
 public class DriveSubsystem extends SubsystemBase {
-  private final TalonFX m_leftMotor  = new TalonFX(DrivetrainConstants.kLeftMotorCANID);
-  private final TalonFX m_rightMotor = new TalonFX(DrivetrainConstants.kRightMotorCANID);
+  private TalonFX m_rightMotor;
+  private TalonFX m_optionalRightMotor;
+
+  private TalonFX m_leftMotor;
+  private TalonFX m_optionalLeftMotor; 
+
   private DifferentialDrive m_Drivetrain;
 
+  private double m_leftSpeed  = 0.0;
+  private double m_rightSpeed = 0.0;
+
+  private final DifferentialDriveKinematics m_kinematics;
+  private final DifferentialDrivePoseEstimator m_poseEstimator;
+  private final AHRS m_gyro;
+
   /** Creates a new DriveSubsystem. */
-  public DriveSubsystem() { 
+  public DriveSubsystem(DifferentialDrivePoseEstimator poseEstimator, DifferentialDriveKinematics kinematics, AHRS gyro, TalonFX rightMotor, TalonFX leftMotor) { 
+    m_poseEstimator = poseEstimator;
+    m_kinematics = kinematics;
+    m_gyro = gyro;
+    m_rightMotor = rightMotor;
+    m_leftMotor = leftMotor;
+
     // Right Motor
-    final MotorOutputConfigs m_rightMotorOutputConfigs = new MotorOutputConfigs();
-    m_rightMotorOutputConfigs.Inverted = InvertedValue.Clockwise_Positive; // why is it a enum :sob:
-    m_rightMotorOutputConfigs.NeutralMode = NeutralModeValue.Brake;
+    TalonFXConfiguration rightMotorConfig = new TalonFXConfiguration();
+    rightMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    rightMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
 
-    final TalonFXConfigurator m_rightMotorConfigurator = m_rightMotor.getConfigurator();
-    m_rightMotorConfigurator.apply(m_rightMotorOutputConfigs);
+    rightMotorConfig.Voltage.withPeakForwardVoltage(Volts.of(DrivetrainConstants.PeakVoltage))
+    .withPeakReverseVoltage(Volts.of(-DrivetrainConstants.PeakVoltage));
+
+    m_rightMotor.getConfigurator().apply(rightMotorConfig);    
+    SendableRegistry.setName(m_rightMotor, "DriveSubsystem", "rightMotor");
 
     // Left Motor
-    final MotorOutputConfigs m_leftMotorOutputConfigs = new MotorOutputConfigs();
-    m_leftMotorOutputConfigs.NeutralMode = NeutralModeValue.Brake;
+    TalonFXConfiguration leftMotorConfig = new TalonFXConfiguration();
+    leftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    leftMotorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+
+    leftMotorConfig.Voltage.withPeakForwardVoltage(Volts.of(DrivetrainConstants.PeakVoltage))
+    .withPeakReverseVoltage(Volts.of(-DrivetrainConstants.PeakVoltage));
+
+    m_leftMotor.getConfigurator().apply(leftMotorConfig);
+    SendableRegistry.setName(m_leftMotor, "DriveSubsystem", "leftMotor");
+
+    // Zeroing the encoders
+    m_leftMotor.setPosition(0);
+    m_rightMotor.setPosition(0);
+
+    // Setting up the drive train
+    m_Drivetrain = new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
+    SendableRegistry.setName(m_Drivetrain, "DriveSubsystem", "Drivetrain");   
+  }
+
+  public void setFollowers(TalonFX optionalRight, TalonFX optionalLeft) {
+    m_optionalRightMotor = optionalRight;
+  
+    // Setting up Config
+    TalonFXConfiguration optionalRightMotorConfig = new TalonFXConfiguration();
+    optionalRightMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    optionalRightMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+
+    optionalRightMotorConfig.Voltage.withPeakForwardVoltage(Volts.of(DrivetrainConstants.PeakVoltage))
+    .withPeakReverseVoltage(Volts.of(-DrivetrainConstants.PeakVoltage));
+
+    // Saving
+    m_optionalRightMotor.getConfigurator().apply(optionalRightMotorConfig);
+
+    // Setting as a follwer
+    m_optionalRightMotor.setControl(
+      new Follower(m_rightMotor.getDeviceID(), false)
+    );
+
+    m_optionalLeftMotor = optionalLeft;
+
+    // Setting up Config
+    TalonFXConfiguration optionalLeftMotorConfig = new TalonFXConfiguration();
+    optionalLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    optionalLeftMotorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+
+    optionalLeftMotorConfig.Voltage.withPeakForwardVoltage(Volts.of(DrivetrainConstants.PeakVoltage))
+    .withPeakReverseVoltage(Volts.of(-DrivetrainConstants.PeakVoltage));
 
-    final TalonFXConfigurator m_leftMotorConfigurator = m_leftMotor.getConfigurator();
-    m_leftMotorConfigurator.apply(m_leftMotorOutputConfigs);
+    // Saving
+    m_optionalLeftMotor.getConfigurator().apply(optionalLeftMotorConfig);
 
-    // TODO: Figure out how to make a differentialDrive w/ these motors
+    // Setting as follower
+    m_optionalLeftMotor.setControl(
+      new Follower(m_leftMotor.getDeviceID(), false)
+    );
+  }
+
+  public void initDefaultCommand(CommandXboxController Controller)
+  {
+    setDefaultCommand(new ArcadeDriveCommand(this, Controller));
+  }
+
+  public void setArcadeSpeeds(double joystickInput, double rotationInput)
+  {
+    m_leftSpeed = (joystickInput / DrivetrainConstants.kSpeedDivider);
+    m_rightSpeed = (rotationInput / DrivetrainConstants.kTurnDivider);
+
+    // NOTE: We are making our own custom input modifications
+    //m_leftSpeed = Math.pow(m_leftSpeed, 3);
+    //m_rightSpeed = Math.pow(m_rightSpeed, 3);
+
+    m_Drivetrain.arcadeDrive(m_leftSpeed, m_rightSpeed, true);
+  }
+
+  public double getSpeed(boolean bLeft)
+  {
+    if (bLeft)
+    {
+      return m_leftSpeed;
+    }
+    else
+    {
+      return m_rightSpeed;
+    }
+  }
+
+  // Wrapping robot position inside of getposition
+  public Pose2d getPose2d(){
+    return m_poseEstimator.getEstimatedPosition();
+  }
+
+  // Resets the drivetrains pose estimator to zero.
+  public void resetPose(){
+    m_leftMotor.setPosition(0);
+    m_rightMotor.setPosition(0);
+    m_poseEstimator.resetPose(Pose2d.kZero);
+  }
+
+  // Returns a robot relative ChassisSpeeds object based on the avrg linear velocity
+  // in meters per second and avrg anglear velocity in readians per second
+  // Currently we are asuming that their is no scale for the motors, we cannot find anywhere to set the scale.
+  public ChassisSpeeds getRobotRelativeSpeeds(){
+    // Linear Velocity in meters per second
+    double leftMPS = m_leftMotor.getVelocity().getValueAsDouble() * DrivetrainConstants.kDrivetrainGearRatio * DrivetrainConstants.kWheelCircumfrance;
+    double rightMPS = m_rightMotor.getVelocity().getValueAsDouble() * DrivetrainConstants.kDrivetrainGearRatio * DrivetrainConstants.kWheelCircumfrance;
+
+    // Make wheelSpeeds object from MPS & converts it to chasis speeds
+    DifferentialDriveWheelSpeeds wheelSpeeds = new DifferentialDriveWheelSpeeds(leftMPS, rightMPS);
+    return m_kinematics.toChassisSpeeds(wheelSpeeds);
+  }
+
+  // Gives the drivetrain a new drive command based on a robot relative
+  // chassis speed object.
+  public void driveRobotRelative(ChassisSpeeds relativeChassisSpeed){
+    m_Drivetrain.arcadeDrive(relativeChassisSpeed.vxMetersPerSecond, 
+                            relativeChassisSpeed.omegaRadiansPerSecond);
   }
 
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
+
+    m_poseEstimator.update(
+        m_gyro.getRotation2d(), 
+        m_leftMotor.getPosition().getValueAsDouble(), 
+        m_rightMotor.getPosition().getValueAsDouble());
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/robot/subsystems/LimelightSubsystem.java
new file mode 100644
index 0000000..0008e82
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/LimelightSubsystem.java
@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.LimelightHelpers;
+
+public class LimelightSubsystem extends SubsystemBase {
+  private final DifferentialDrivePoseEstimator m_poseEstimator;
+
+  /** Creates a new PositionSubsystem. */
+  public LimelightSubsystem(DifferentialDrivePoseEstimator poseEstimator) {
+    m_poseEstimator = poseEstimator;
+  }
+
+  /**
+   * An example method querying a boolean state of the subsystem (for example, a digital sensor).
+   *
+   * @return value of some boolean subsystem state, such as a digital sensor.
+   */
+  public boolean exampleCondition() {
+    // Query some boolean state, such as a digital sensor.
+    return false;
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler 
+
+    // This is copy and pasted from limelight's documentation for testing.
+    LimelightHelpers.PoseEstimate limelightMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight");
+    if (limelightMeasurement.tagCount >= 2) {  // Only trust measurement if we see multiple tags
+        m_poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 9999999));
+        m_poseEstimator.addVisionMeasurement(
+            limelightMeasurement.pose,
+            limelightMeasurement.timestampSeconds
+        );
+    }
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    // This method will be called once per scheduler run during simulation
+  }
+}
diff --git a/src/main/java/frc/robot/utils/getAutoNames.java b/src/main/java/frc/robot/utils/getAutoNames.java
new file mode 100644
index 0000000..e864220
--- /dev/null
+++ b/src/main/java/frc/robot/utils/getAutoNames.java
@@ -0,0 +1,42 @@
+package frc.robot.utils;
+
+import java.io.File;
+import java.util.ArrayList;
+
+// This goes to "/home/lvuser/deploy/pathplanner/autos" and grabs the names of all .auto files
+// We are asuming that the PathPlannerAuto(string) is the file name of the auto
+// We are asuming that the files are always in the "/home/lvuser/deploy/pathplanner/autos"
+// We found this information at https://github.com/mjansen4857/pathplanner/wiki/PathPlannerLib:-Java-Usage
+public class getAutoNames {
+    public String[] main() {
+        // Creating an ArrayList so I can fill it with auto file names w/o the extention.
+        ArrayList<String> autoNames = new ArrayList<String>();
+
+        // Getting the folder and files.
+        File folder = new File("/home/lvuser/deploy/pathplanner/autos");
+        File[] listOfFiles = folder.listFiles();
+
+        // Checking if the files exist
+        if (listOfFiles != null) {
+            for (File file : listOfFiles) {
+                if (file.isFile()) {
+                    String fullName = file.getName(); // Returns [file_name].[extention]
+                    String[] splitName = fullName.split("."); // Splitting the file name into seprate parts.
+                    
+                    // If its an auto file then add it to the list.
+                    if (splitName[1] == "auto") {
+                        autoNames.add(splitName[0]);
+                        System.out.println(splitName[0]);
+                    }
+                }
+            }
+        }
+
+        // Converting the ArrayList into a normal array for memory optimization & saftey
+        String[] autoNamesArray = new String[autoNames.size()];
+        autoNames.toArray(autoNamesArray);
+
+        // Returning the new array
+        return autoNamesArray;
+    }
+}
diff --git a/vendordeps/PathplannerLib-2025.2.7.json b/vendordeps/PathplannerLib-2025.2.7.json
new file mode 100644
index 0000000..d3f84e5
--- /dev/null
+++ b/vendordeps/PathplannerLib-2025.2.7.json
@@ -0,0 +1,38 @@
+{
+    "fileName": "PathplannerLib-2025.2.7.json",
+    "name": "PathplannerLib",
+    "version": "2025.2.7",
+    "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
+    "frcYear": "2025",
+    "mavenUrls": [
+        "https://3015rangerrobotics.github.io/pathplannerlib/repo"
+    ],
+    "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-java",
+            "version": "2025.2.7"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-cpp",
+            "version": "2025.2.7",
+            "libName": "PathplannerLib",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal",
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64"
+            ]
+        }
+    ]
+}
\ No newline at end of file
diff --git a/vendordeps/Phoenix6-25.1.0.json b/vendordeps/Phoenix6-frc2025-latest.json
similarity index 75%
rename from vendordeps/Phoenix6-25.1.0.json
rename to vendordeps/Phoenix6-frc2025-latest.json
index 473f6a8..6f40c84 100644
--- a/vendordeps/Phoenix6-25.1.0.json
+++ b/vendordeps/Phoenix6-frc2025-latest.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "Phoenix6-25.1.0.json",
+    "fileName": "Phoenix6-frc2025-latest.json",
     "name": "CTRE-Phoenix (v6)",
-    "version": "25.1.0",
+    "version": "25.4.0",
     "frcYear": "2025",
     "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
     "mavenUrls": [
@@ -19,14 +19,14 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-java",
-            "version": "25.1.0"
+            "version": "25.4.0"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "api-cpp",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -40,7 +40,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -54,7 +54,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "api-cpp-sim",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -68,7 +68,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -82,7 +82,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -96,7 +96,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -110,7 +110,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -124,7 +124,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -138,7 +138,21 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.1.0",
+            "version": "25.4.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFXS",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -152,7 +166,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -166,7 +180,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -180,7 +194,35 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.1.0",
+            "version": "25.4.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANdi",
+            "version": "25.4.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANdle",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -196,7 +238,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-cpp",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_Phoenix6_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -212,7 +254,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_PhoenixTools",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -228,7 +270,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "wpiapi-cpp-sim",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_Phoenix6_WPISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -244,7 +286,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_PhoenixTools_Sim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -260,7 +302,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_SimTalonSRX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -276,7 +318,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_SimVictorSPX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -292,7 +334,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_SimPigeonIMU",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -308,7 +350,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_SimCANCoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -324,7 +366,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_SimProTalonFX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -337,10 +379,26 @@
             ],
             "simMode": "swsim"
         },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFXS",
+            "version": "25.4.0",
+            "libName": "CTRE_SimProTalonFXS",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_SimProCANcoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -356,7 +414,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_SimProPigeon2",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -372,7 +430,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_SimProCANrange",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -384,6 +442,38 @@
                 "osxuniversal"
             ],
             "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANdi",
+            "version": "25.4.0",
+            "libName": "CTRE_SimProCANdi",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANdle",
+            "version": "25.4.0",
+            "libName": "CTRE_SimProCANdle",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
         }
     ]
 }
\ No newline at end of file
diff --git a/vendordeps/REVLib-2025.0.0.json b/vendordeps/REVLib.json
similarity index 86%
rename from vendordeps/REVLib-2025.0.0.json
rename to vendordeps/REVLib.json
index cde6011..ac62be8 100644
--- a/vendordeps/REVLib-2025.0.0.json
+++ b/vendordeps/REVLib.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "REVLib-2025.0.0.json",
+    "fileName": "REVLib.json",
     "name": "REVLib",
-    "version": "2025.0.0",
+    "version": "2025.0.3",
     "frcYear": "2025",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
@@ -12,19 +12,18 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-java",
-            "version": "2025.0.0"
+            "version": "2025.0.3"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.0",
+            "version": "2025.0.3",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
@@ -37,14 +36,13 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-cpp",
-            "version": "2025.0.0",
+            "version": "2025.0.3",
             "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
             "binaryPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
@@ -55,14 +53,13 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.0",
+            "version": "2025.0.3",
             "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
             "binaryPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
diff --git a/vendordeps/Studica-2025.0.1.json b/vendordeps/Studica-2025.0.1.json
new file mode 100644
index 0000000..5010be0
--- /dev/null
+++ b/vendordeps/Studica-2025.0.1.json
@@ -0,0 +1,71 @@
+{
+    "fileName": "Studica-2025.0.1.json",
+    "name": "Studica",
+    "version": "2025.0.1",
+    "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
+    "frcYear": "2025",
+    "mavenUrls": [
+        "https://dev.studica.com/maven/release/2025/"
+    ],
+    "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.1.json",
+    "cppDependencies": [
+        {
+            "artifactId": "Studica-cpp",
+            "binaryPlatforms": [
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64",
+                "linuxx86-64",
+                "osxuniversal",
+                "windowsx86-64"
+            ],
+            "groupId": "com.studica.frc",
+            "headerClassifier": "headers",
+            "libName": "Studica",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "version": "2025.0.1"
+        },
+        {
+            "artifactId": "Studica-driver",
+            "binaryPlatforms": [
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64",
+                "linuxx86-64",
+                "osxuniversal",
+                "windowsx86-64"
+            ],
+            "groupId": "com.studica.frc",
+            "headerClassifier": "headers",
+            "libName": "StudicaDriver",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "version": "2025.0.1"
+        }
+    ],
+    "javaDependencies": [
+        {
+            "artifactId": "Studica-java",
+            "groupId": "com.studica.frc",
+            "version": "2025.0.1"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "artifactId": "Studica-driver",
+            "groupId": "com.studica.frc",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64",
+                "linuxx86-64",
+                "osxuniversal",
+                "windowsx86-64"
+            ],
+            "version": "2025.0.1"
+        }
+    ]
+}
\ No newline at end of file