diff --git a/build.gradle b/build.gradle
index e7cb87ed..a07b1d7f 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,7 +1,7 @@
 import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO
 
 plugins {
-	id "idea"
+	id "java"
 	id "org.jetbrains.kotlin.jvm" version "1.6.10"
 	id "edu.wpi.first.GradleRIO" version "2023.4.2"
 	id "com.diffplug.spotless" version "6.3.0"
@@ -155,6 +155,10 @@ compileKotlin {
 	}
 }
 
+compileJava{
+
+}
+
 gversion {
 	srcDir       = "src/main/kotlin/"           // path is relative to the sub-project by default
 	// Gradle variables can also be used
@@ -172,6 +176,7 @@ gversion {
 
 
 project.compileKotlin.dependsOn(createVersionFile)
+project.compileJava.dependsOn(createVersionFile)
 // Configure jar and deploy tasks
 deployArtifact.jarTask = jar
 wpi.java.configureExecutableTasks(jar)
diff --git a/src/main/java/com/team4099/utils/LimelightHelpers.java b/src/main/java/com/team4099/utils/LimelightHelpers.java
new file mode 100644
index 00000000..0cb199a3
--- /dev/null
+++ b/src/main/java/com/team4099/utils/LimelightHelpers.java
@@ -0,0 +1,779 @@
+package com.team4099.utils;//com.team4099.utils.LimelightHelpers v1.2.1 (March 1, 2023)
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+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.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;
+
+public class LimelightHelpers {
+
+    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("txp")
+        public double tx_pixels;
+
+        @JsonProperty("ty")
+        public double ty;
+
+        @JsonProperty("typ")
+        public double ty_pixels;
+
+        @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];
+        }
+
+    }
+
+    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("txp")
+        public double tx_pixels;
+
+        @JsonProperty("ty")
+        public double ty;
+
+        @JsonProperty("typ")
+        public double ty_pixels;
+
+        @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];
+        }
+    }
+
+    public static class LimelightTarget_Barcode {
+
+    }
+
+    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() {
+        }
+    }
+
+    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("txp")
+        public double tx_pixels;
+
+        @JsonProperty("ty")
+        public double ty;
+
+        @JsonProperty("typ")
+        public double ty_pixels;
+
+        public LimelightTarget_Detector() {
+        }
+    }
+
+    public static class Results {
+
+        @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("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 Results() {
+            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];
+
+        }
+    }
+
+    public static class LimelightResults {
+        @JsonProperty("Results")
+        public Results targetingResults;
+
+        public LimelightResults() {
+            targetingResults = new Results();
+        }
+    }
+
+    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;
+    }
+
+    private 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])));
+    }
+
+    private 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);
+    }
+
+    public static NetworkTable getLimelightNTTable(String tableName) {
+        return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName));
+    }
+
+    public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) {
+        return getLimelightNTTable(tableName).getEntry(entryName);
+    }
+
+    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 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;
+    }
+    /////
+    /////
+
+    public static double getTX(String limelightName) {
+        return getLimelightNTDouble(limelightName, "tx");
+    }
+
+    public static double getTY(String limelightName) {
+        return getLimelightNTDouble(limelightName, "ty");
+    }
+
+    public static double getTA(String limelightName) {
+        return getLimelightNTDouble(limelightName, "ta");
+    }
+
+    public static double getLatency_Pipeline(String limelightName) {
+        return getLimelightNTDouble(limelightName, "tl");
+    }
+
+    public static double getLatency_Capture(String limelightName) {
+        return getLimelightNTDouble(limelightName, "cl");
+    }
+
+    public static double getCurrentPipelineIndex(String limelightName) {
+        return getLimelightNTDouble(limelightName, "getpipe");
+    }
+
+    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 double getNeuralClassID(String limelightName) {
+        return getLimelightNTDouble(limelightName, "tclass");
+    }
+
+    /////
+    /////
+
+    public static Pose3d getBotPose3d(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose");
+        return toPose3D(poseArray);
+    }
+
+    public static Pose3d getBotPose3d_wpiRed(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired");
+        return toPose3D(poseArray);
+    }
+
+    public static Pose3d getBotPose3d_wpiBlue(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue");
+        return toPose3D(poseArray);
+    }
+
+    public static Pose3d getBotPose3d_TargetSpace(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace");
+        return toPose3D(poseArray);
+    }
+
+    public static Pose3d getCameraPose3d_TargetSpace(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace");
+        return toPose3D(poseArray);
+    }
+
+    public static Pose3d getTargetPose3d_CameraSpace(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace");
+        return toPose3D(poseArray);
+    }
+
+    public static Pose3d getTargetPose3d_RobotSpace(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace");
+        return toPose3D(poseArray);
+    }
+
+    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 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 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);
+
+    }
+
+    public static boolean getTV(String limelightName) {
+        return 1.0 == getLimelightNTDouble(limelightName, "tv");
+    }
+
+    /////
+    /////
+
+    public static void setPipelineIndex(String limelightName, int pipelineIndex) {
+        setLimelightNTDouble(limelightName, "pipeline", pipelineIndex);
+    }
+
+    /**
+     * The LEDs will be controlled by Limelight pipeline settings, and not by robot
+     * code.
+     */
+    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);
+    }
+
+    public static void setStreamMode_Standard(String limelightName) {
+        setLimelightNTDouble(limelightName, "stream", 0);
+    }
+
+    public static void setStreamMode_PiPMain(String limelightName) {
+        setLimelightNTDouble(limelightName, "stream", 1);
+    }
+
+    public static void setStreamMode_PiPSecondary(String limelightName) {
+        setLimelightNTDouble(limelightName, "stream", 2);
+    }
+
+    public static void setCameraMode_Processor(String limelightName) {
+        setLimelightNTDouble(limelightName, "camMode", 0);
+    }
+    public static void setCameraMode_Driver(String limelightName) {
+        setLimelightNTDouble(limelightName, "camMode", 1);
+    }
+
+
+    /**
+     * Sets the crop window. The crop window in the UI must be completely open for
+     * dynamic cropping to work.
+     */
+    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);
+    }
+
+    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;
+    }
+
+    /**
+     * Parses Limelight's JSON results dump into a LimelightResults Object
+     */
+    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) {
+            System.err.println("lljson error: " + e.getMessage());
+        }
+
+        long end = System.nanoTime();
+        double millis = (end - start) * .000001;
+        results.targetingResults.latency_jsonParse = millis;
+        if (profileJSON) {
+            System.out.printf("lljson: %.2f\r\n", millis);
+        }
+
+        return results;
+    }
+}
+
diff --git a/src/main/kotlin/com/team4099/lib/vision/TargetCorner.kt b/src/main/kotlin/com/team4099/lib/vision/TargetCorner.kt
index 0c4a0d7b..fc742384 100644
--- a/src/main/kotlin/com/team4099/lib/vision/TargetCorner.kt
+++ b/src/main/kotlin/com/team4099/lib/vision/TargetCorner.kt
@@ -1,9 +1,14 @@
 package com.team4099.lib.vision
 
+import com.team4099.robot2023.subsystems.limelight.CoordinatePair
 import org.photonvision.targeting.TargetCorner
 
 data class TargetCorner(val x: Double, val y: Double) {
   constructor(
     photonTargetCorner: TargetCorner
   ) : this(photonTargetCorner.x, photonTargetCorner.y) {}
+
+  fun toCoordinatePair(): CoordinatePair {
+    return CoordinatePair(x, y)
+  }
 }
diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
index 403e13af..361a6f2f 100644
--- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
+++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -12,18 +12,24 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim
 import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO
 import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIOPigeon2
 import com.team4099.robot2023.subsystems.elevator.Elevator
+import com.team4099.robot2023.subsystems.elevator.ElevatorIO
 import com.team4099.robot2023.subsystems.elevator.ElevatorIONeo
 import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim
 import com.team4099.robot2023.subsystems.gameboy.GameBoy
 import com.team4099.robot2023.subsystems.gameboy.GameboyIOServer
 import com.team4099.robot2023.subsystems.gameboy.objective.isConeNode
 import com.team4099.robot2023.subsystems.groundintake.GroundIntake
+import com.team4099.robot2023.subsystems.groundintake.GroundIntakeIO
 import com.team4099.robot2023.subsystems.groundintake.GroundIntakeIONeo
 import com.team4099.robot2023.subsystems.groundintake.GroundIntakeIOSim
 import com.team4099.robot2023.subsystems.led.Led
 import com.team4099.robot2023.subsystems.led.LedIOCandle
 import com.team4099.robot2023.subsystems.led.LedIOSim
+import com.team4099.robot2023.subsystems.limelight.LimelightVision
+import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO
+import com.team4099.robot2023.subsystems.limelight.LimelightVisionIOReal
 import com.team4099.robot2023.subsystems.manipulator.Manipulator
+import com.team4099.robot2023.subsystems.manipulator.ManipulatorIO
 import com.team4099.robot2023.subsystems.manipulator.ManipulatorIONeo
 import com.team4099.robot2023.subsystems.manipulator.ManipulatorIOSim
 import com.team4099.robot2023.subsystems.superstructure.Request
@@ -39,6 +45,7 @@ object RobotContainer {
   private val drivetrain: Drivetrain
   private val vision: Vision
   private val superstructure: Superstructure
+  private val limelight: LimelightVision
 
   init {
     if (RobotBase.isReal()) {
@@ -53,12 +60,13 @@ object RobotContainer {
         )
       superstructure =
         Superstructure(
-          Elevator(ElevatorIONeo),
-          GroundIntake(GroundIntakeIONeo),
-          Manipulator(ManipulatorIONeo),
+          Elevator(object: ElevatorIO {}),
+          GroundIntake(object: GroundIntakeIO {}),
+          Manipulator(object: ManipulatorIO {}),
           Led(LedIOCandle),
           GameBoy(GameboyIOServer)
         )
+      limelight = LimelightVision(LimelightVisionIOReal())
     } else {
       // Simulation implementations
       drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim)
@@ -71,6 +79,7 @@ object RobotContainer {
           Led(LedIOSim),
           GameBoy(GameboyIOServer)
         )
+      limelight = LimelightVision(object : LimelightVisionIO {})
     }
 
     vision.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) })
@@ -167,7 +176,7 @@ object RobotContainer {
     ControlBoard.scoreOuttake.whileTrue(superstructure.score())
     ControlBoard.singleSubstationIntake.whileTrue(superstructure.singleSubConeCommand())
     ControlBoard.groundIntakeCube.whileTrue(superstructure.groundIntakeCubeCommand())
-
+    ControlBoard.doubleSubstationIntake.whileTrue(superstructure.doubleSubConeCommand())
     ControlBoard.prepScore.whileTrue(
       superstructure.prepScoreCommand(
         {
@@ -180,7 +189,6 @@ object RobotContainer {
 
     ControlBoard.groundIntakeCone.whileTrue(superstructure.groundIntakeConeCommand())
     ControlBoard.dpadUp.whileTrue(AutoScoreCommand(drivetrain, superstructure))
-    ControlBoard.singleSubIntake.whileTrue(superstructure.singleSubConeCommand())
     //    ControlBoard.dpadDown.whileTrue(PickupFromSubstationCommand(drivetrain, superstructure))
 
     //    ControlBoard.doubleSubstationIntake.whileTrue(AutoScoreCommand(drivetrain,
diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt
index c84eb15e..69f46cc4 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt
@@ -54,7 +54,6 @@ object ControlBoard {
   val singleSubstationIntake = Trigger { driver.bButton }
   val scoreOuttake = Trigger { driver.xButton }
   val groundIntakeCube = Trigger { driver.rightShoulderButton }
-  val singleSubIntake = Trigger { driver.leftShoulderButton }
 
   val increaseRollerVoltage = Trigger { operator.dPadUp }
   val decreaseRollerVoltage = Trigger { operator.dPadDown }
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt
index 181272be..4cfa010b 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt
@@ -12,7 +12,7 @@ typealias NodeTier = Constants.Universal.NodeTier
 object Constants {
   object Universal {
     val SIM_MODE = Tuning.SimType.SIM
-    const val REAL_FIELD = true
+    const val REAL_FIELD = false
 
     const val CTRE_CONFIG_TIMEOUT = 0
     const val EPSILON = 1E-9
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt
index 38a0203e..accec206 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt
@@ -14,8 +14,6 @@ import org.team4099.lib.units.base.pounds
 import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.derived.cos
 import org.team4099.lib.units.derived.degrees
-import org.team4099.lib.units.derived.driven
-import org.team4099.lib.units.derived.driving
 import org.team4099.lib.units.derived.gearRatio
 import org.team4099.lib.units.derived.sin
 import org.team4099.lib.units.derived.volts
@@ -25,7 +23,7 @@ import kotlin.math.PI
 object ElevatorConstants {
 
   const val SENSOR_CPR = 42
-  val GEAR_RATIO = (24.0.driven / 12.0.driving).gearRatio
+  val GEAR_RATIO = ((58.0 / 14.0) * (84.0 / 58.0) * (28.0 / 84.0)).gearRatio
   val CARRIAGE_MASS = 10.pounds
 
   const val FOLLOW_MOTOR_INVERTED = true
@@ -69,8 +67,8 @@ object ElevatorConstants {
   // circumference / 2pi = radius
   val SPOOL_RADIUS = 0.005.meters * 32.0 / (2 * PI)
 
-  val MAX_VELOCITY = 150.inches.perSecond // 75
-  val MAX_ACCELERATION = 500.inches.perSecond.perSecond // 225
+  val MAX_VELOCITY = 75.inches.perSecond // 75
+  val MAX_ACCELERATION = 225.inches.perSecond.perSecond // 225
 
   val ELEVATOR_MAX_EXTENSION = 54.8.inches
   val ELEVATOR_MAX_RETRACTION = 0.0.inches
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt
index 3d5e877b..5bb3ba81 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt
@@ -47,7 +47,7 @@ object FieldConstants {
         2,
         Pose3d(
           40.inches,
-          (42.125).inches,
+          (104.125).inches,
           (18.22).inches,
           Rotation3d(0.0.radians, 0.0.radians, 0.0.radians)
         )
@@ -55,10 +55,10 @@ object FieldConstants {
       AprilTag(
         3,
         Pose3d(
-          (610.77).inches,
-          (174.19).inches, // FIRST's diagram has a typo (it says 147.19)
+          40.inches,
+          (42.125).inches,
           (18.22).inches,
-          Rotation3d(0.0.radians, 0.0.radians, Math.PI.radians)
+          Rotation3d(0.0.radians, 0.0.radians, 0.0.radians)
         )
       ),
       AprilTag(
@@ -73,8 +73,8 @@ object FieldConstants {
       AprilTag(
         5,
         Pose3d(
-          (415.5).inches,
-          (124.5).inches,
+          (409.5).inches,
+          (85.5).inches,
           (27.833).inches,
           Rotation3d(0.0.radians, 0.0.radians, Math.PI.radians)
         )
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ManipulatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ManipulatorConstants.kt
index 126e5022..48a7972a 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/ManipulatorConstants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ManipulatorConstants.kt
@@ -130,7 +130,7 @@ object ManipulatorConstants {
   val MIN_EXTENSION = 1.0.inches
   val SINGLE_SUBSTATION_INTAKE_EXTENSION = 1.0.inches
   val DOUBLE_SUBSTATION_SHELF_INTAKE_EXTENSION = 7.0.inches
-  val LOW_CUBE_SCORE_EXTENSION = 7.5.inches
+  val LOW_CUBE_SCORE_EXTENSION = 1.2.inches
   val MID_CUBE_SCORE_EXTENSION = 3.0.inches
   val HIGH_CUBE_SCORE_EXTENSION = 8.6.inches
   val LOW_CONE_SCORE_EXTENSION = 7.5.inches
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt
index afdb6b7e..f18a116f 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt
@@ -25,4 +25,17 @@ object VisionConstants {
     )
 
   val CAMERA_NAMES = listOf("northstar")
+
+  object Limelight {
+    val LIMELIGHT_NAME = "limelight-zapdos"
+    val HORIZONTAL_FOV = 59.6.degrees
+    val VERITCAL_FOV = 45.7.degrees
+    val LL_TRANSFORM =
+      Transform3d(
+        Translation3d(-1.1438.inches, 10.3966.inches, 12.9284.inches),
+        Rotation3d(0.0.degrees, 61.610.degrees, 16.1525.degrees)
+      )
+    const val RES_WIDTH = 320
+    const val RES_HEIGHT = 240 // no clue what these numbers should be but usnig these for now
+  }
 }
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt
index e602ab6b..04f379c4 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt
@@ -159,6 +159,11 @@ class Elevator(val io: ElevatorIO) {
         "Elevator/hybridHeight", 16.0.inches, Pair({ it.inInches }, { it.inches })
       )
 
+    val cubeHybridHeight =
+      LoggedTunableValue(
+        "Elevator/hybridHeight", 2.8.inches, Pair({ it.inInches }, { it.inches })
+      )
+
     val midCubeHeight =
       LoggedTunableValue(
         "Elevator/midCubeHeight",
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt
index 1adb8c21..798f2668 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt
@@ -53,7 +53,7 @@ interface ElevatorIO {
       table?.put("elevatorFollowerTempCelsius", followerTempCelcius.inCelsius)
     }
 
-    override fun fromLog(table: LogTable) {
+    override fun fromLog(table: LogTable?) {
       table?.getDouble("elevatorPositionInches", elevatorPosition.inInches)?.let {
         elevatorPosition = it.inches
       }
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONeo.kt
index 7de4d285..f2815dd1 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONeo.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONeo.kt
@@ -6,6 +6,7 @@ import com.revrobotics.SparkMaxPIDController
 import com.team4099.lib.math.clamp
 import com.team4099.robot2023.config.constants.Constants
 import com.team4099.robot2023.config.constants.ElevatorConstants
+import org.littletonrobotics.junction.Logger
 import org.team4099.lib.units.base.Length
 import org.team4099.lib.units.base.Meter
 import org.team4099.lib.units.base.amps
@@ -100,6 +101,9 @@ object ElevatorIONeo : ElevatorIO {
     inputs.followerSupplyCurrent = inputs.followerStatorCurrent * followerSparkMax.appliedOutput
 
     inputs.followerTempCelcius = followerSparkMax.motorTemperature.celsius
+
+    Logger.getInstance()
+      .recordOutput("Elevator/leaderRawRotations", leaderSparkMax.encoder.position)
   }
 
   /**
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/CoordinatePair.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/CoordinatePair.kt
new file mode 100644
index 00000000..44c95e69
--- /dev/null
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/CoordinatePair.kt
@@ -0,0 +1,3 @@
+package com.team4099.robot2023.subsystems.limelight
+
+data class CoordinatePair(val x: Double, val y: Double)
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt
new file mode 100644
index 00000000..814238c4
--- /dev/null
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt
@@ -0,0 +1,170 @@
+package com.team4099.robot2023.subsystems.limelight
+
+import com.team4099.lib.vision.TargetCorner
+import com.team4099.robot2023.config.constants.VisionConstants
+import com.team4099.robot2023.util.PoseEstimator
+import com.team4099.robot2023.util.rotateBy
+import com.team4099.robot2023.util.toPose3d
+import com.team4099.robot2023.util.toTransform3d
+import edu.wpi.first.wpilibj2.command.SubsystemBase
+import org.littletonrobotics.junction.Logger
+import org.team4099.lib.geometry.Pose2d
+import org.team4099.lib.geometry.Pose3d
+import org.team4099.lib.geometry.Rotation3d
+import org.team4099.lib.geometry.Transform3d
+import org.team4099.lib.geometry.Translation3d
+import org.team4099.lib.units.base.Length
+import org.team4099.lib.units.base.inches
+import org.team4099.lib.units.base.meters
+import org.team4099.lib.units.derived.Angle
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.tan
+import java.util.function.Consumer
+import java.util.function.Supplier
+
+class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() {
+  val inputs = LimelightVisionIO.LimelightVisionIOInputs()
+
+  var poseSupplier: Supplier<Pose2d> = Supplier { Pose2d() }
+  var visionConsumer: Consumer<List<PoseEstimator.TimestampedVisionUpdate>> = Consumer {}
+
+  // i think we need this for camera project to irl coordinates
+  val vpw = (2.0 * (VisionConstants.Limelight.HORIZONTAL_FOV / 2).tan)
+  val vph = (2.0 * (VisionConstants.Limelight.VERITCAL_FOV / 2).tan)
+
+  override fun periodic() {
+    io.updateInputs(inputs)
+    Logger.getInstance().processInputs("LimelightVision", inputs)
+
+    val currentPose: Pose2d = poseSupplier.get()
+
+    val visibleNodes = mutableListOf<Pose3d>()
+
+    // mathematically figure out which nodes we're looking at based on limelight fov
+
+    // process transform outputs from LL and get the target corners
+    if (inputs.validReading) {
+
+      for (target in inputs.retroTargets) {
+        visibleNodes.add(
+          solveTargetPositionFromAngularOutput(
+            target.tx,
+            target.ty,
+            currentPose,
+            VisionConstants.Limelight.LL_TRANSFORM,
+            if (target.tyPixel < 100) 23.905.inches else 43.875.inches
+            // 100 is pixel pos between mid and high tape, pls tune
+        ))
+      }
+
+      // this is adding where we think they are,, not where they actually are
+    }
+
+    Logger.getInstance().recordOutput("LimelightVision/visibleNodes", *visibleNodes.map { it.pose3d }.toTypedArray())
+  }
+
+  // based off of angles
+  fun solveTargetPositionFromAngularOutput(
+    tx: Angle,
+    ty: Angle,
+    currentPose: Pose2d,
+    cameraTransform: Transform3d,
+    targetHeight: Length
+  ): Pose3d {
+    val horizontalAngleFromCamera = tx
+    val verticalAngleFromCamera = ty
+
+    // rotation from robot to the target in frame
+    val rotationFromTargetToRobot =
+      Rotation3d(0.0.degrees, verticalAngleFromCamera, horizontalAngleFromCamera)
+
+    val xDistanceFromTargetToRobot = (targetHeight - cameraTransform.z) / verticalAngleFromCamera.tan
+    val yDistanceFromTargetToRobot = xDistanceFromTargetToRobot / horizontalAngleFromCamera.tan
+
+    val translationFromTargetToRobot =
+      Translation3d(
+        xDistanceFromTargetToRobot,
+        yDistanceFromTargetToRobot,
+        targetHeight - cameraTransform.z
+      )
+
+    return currentPose
+      .toPose3d()
+      .transformBy(cameraTransform.toPose3d().transformBy(Transform3d(translationFromTargetToRobot, rotationFromTargetToRobot).inverse()).toTransform3d().inverse())
+
+  }
+
+  // based off of pixel coordinates
+  private fun pixelCoordsToNormalizedPixelCoords(pixelCoords: CoordinatePair): CoordinatePair {
+    // we're defining a coordinate pair to be where 0,0 is (horizontal fov / 2, vertical fov / 2)
+    // and 1,1 is 1 pixel shifted both vertically and horizontally
+    // note that pixel coordinates is returned where 0,0 is upper left, pos x is down, and pos y is
+    // right but we want pos x to be right and pos y to be up
+    return CoordinatePair(
+      1 / (VisionConstants.Limelight.RES_WIDTH / 2) *
+        (pixelCoords.x - VisionConstants.Limelight.RES_WIDTH - 0.5),
+      1 / (VisionConstants.Limelight.RES_HEIGHT / 2) *
+        (pixelCoords.x - VisionConstants.Limelight.RES_HEIGHT - 0.5)
+    )
+  }
+
+  // need to make sure that all the corners are from the same target before passing into this
+  // function
+  fun solveTargetPositionFromCameraOutput(
+    currentPose: Pose2d,
+    pixelPosition: CoordinatePair,
+    cameraTransform: Transform3d,
+    targetHeight: Length
+  ): Pose3d {
+    val normalizedCoordinates = pixelCoordsToNormalizedPixelCoords(pixelPosition)
+
+    // figure out which way this target is facing using yaw of robot and yaw of camera
+    val targetRotation =
+      if (currentPose.rotation.rotateBy(cameraTransform.rotation.z) in 0.degrees..180.degrees) {
+        // we are looking at a blue node which is facing towards 0 degrees
+        0.0.degrees
+      } else {
+        // we are looking at a red node which is facing 180 degrees
+        180.degrees
+      }
+
+    // i think we wanna do what is there above because we are getting absolute position of the node
+    // relative to the origin
+    //    val targetRotation =
+    // currentPose.rotation.rotateBy(VisionConstants.Limelight.LL_TRANSFORM.rotation.z).rotateBy(180.degrees) // assuming we are looking at the tape head on
+
+    val cartesianPoseOfTarget =
+      Pose3d(
+        currentPose.x + (vpw / 2 * normalizedCoordinates.x).meters,
+        currentPose.y + (vph / 2 * normalizedCoordinates.y).meters,
+        targetHeight,
+        Rotation3d(0.0.degrees, 0.0.degrees, targetRotation)
+      )
+
+    return cartesianPoseOfTarget
+  }
+
+  // assume top left, top right, bottom left, bottom right
+  fun centerOfRectangle(corners: List<TargetCorner>): CoordinatePair? {
+    var xPos: Double? = null
+    var yPos: Double? = null
+    if (corners.size == 4) {
+      xPos = corners.map { it.x }.average()
+      yPos = corners.map { it.y }.average()
+    }
+
+    return if (xPos != null && yPos != null) {
+      CoordinatePair(xPos, yPos)
+    } else {
+      null
+    }
+  }
+
+  fun setDataInterfaces(
+    poseSupplier: Supplier<Pose2d>,
+    visionConsumer: Consumer<List<PoseEstimator.TimestampedVisionUpdate>>
+  ) {
+    this.poseSupplier = poseSupplier
+    this.visionConsumer = visionConsumer
+  }
+}
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIO.kt
new file mode 100644
index 00000000..04738c45
--- /dev/null
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIO.kt
@@ -0,0 +1,71 @@
+package com.team4099.robot2023.subsystems.limelight
+
+import com.team4099.robot2023.util.LimelightReading
+import org.littletonrobotics.junction.LogTable
+import org.littletonrobotics.junction.inputs.LoggableInputs
+import org.team4099.lib.units.base.inSeconds
+import org.team4099.lib.units.base.seconds
+import org.team4099.lib.units.derived.Angle
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inDegrees
+
+interface LimelightVisionIO {
+
+  class LimelightVisionIOInputs : LoggableInputs {
+    var timestamp = 0.0.seconds
+    var fps = 0.0
+    var validReading = false
+    var angle = 0.degrees
+    var retroTargets = listOf<LimelightReading>()
+
+    override fun fromLog(table: LogTable?) {
+      table?.getDouble("timestampSeconds", timestamp.inSeconds)?.let { timestamp = it.seconds }
+      table?.getDouble("fps", fps)?.let { fps = it }
+      table?.getBoolean("validReading", validReading)?.let { validReading = it }
+      table?.getDouble("simpleAngleDegrees", angle.inDegrees)?.let { angle = it.degrees }
+      val numOfTargets = table?.getInteger("numOfTargets", 0) ?: 0
+      val retrievedTargets = mutableListOf<LimelightReading>()
+      for (targetIndex in 0 until numOfTargets) {
+        val targetTx: Angle? = table?.getDouble("Detection/$targetIndex/tx", 0.0)?.degrees
+        val targetTy: Angle? = table?.getDouble("Detection/$targetIndex/tx", 0.0)?.degrees
+        val targetTxPixels: Double? = table?.getDouble("Detection/$targetIndex/txPixels", 0.0)
+        val targetTyPixels: Double? = table?.getDouble("Detection/$targetIndex/tyPixels", 0.0)
+        val targetTs: Angle? = table?.getDouble("Detection/$targetIndex/tyPixels", 0.0)?.degrees
+        if (targetTx != null &&
+          targetTy != null &&
+          targetTxPixels != null &&
+          targetTyPixels != null &&
+          targetTs != null
+        ) {
+          retrievedTargets.add(
+            LimelightReading(targetTx, targetTy, targetTxPixels, targetTyPixels, targetTs)
+          )
+        }
+      }
+      retroTargets = retrievedTargets.toList()
+    }
+
+    override fun toLog(table: LogTable?) {
+      table?.put("timestampSeconds", timestamp.inSeconds)
+      table?.put("fps", fps)
+      table?.put("validReading", validReading)
+      table?.put("simpleAngleDegrees", angle.inDegrees)
+      table?.put("numOfTargets", retroTargets.size.toLong())
+      table?.put("cornersX", retroTargets.map { it.txPixel }.toDoubleArray())
+      table?.put("cornersY", retroTargets.map { it.tyPixel }.toDoubleArray())
+      for (i in retroTargets.indices) {
+        table?.put("Detection/$i/txDegrees", retroTargets[i].tx.inDegrees)
+        table?.put("Detection/$i/tyDegrees", retroTargets[i].ty.inDegrees)
+        table?.put("Detection/$i/tyPixels", retroTargets[i].tyPixel)
+        table?.put("Detection/$i/txPixels", retroTargets[i].txPixel)
+        table?.put("Detection/$i/tsDegrees", retroTargets[i].ts.inDegrees)
+      }
+    }
+  }
+
+  fun updateInputs(inputs: LimelightVisionIOInputs) {}
+
+  fun setPipeline(pipelineIndex: Int) {}
+
+  fun setLeds(enabled: Boolean) {}
+}
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOReal.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOReal.kt
new file mode 100644
index 00000000..9619ffd0
--- /dev/null
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOReal.kt
@@ -0,0 +1,56 @@
+package com.team4099.robot2023.subsystems.limelight
+
+import com.team4099.lib.hal.Clock
+import com.team4099.robot2023.config.constants.VisionConstants.Limelight.LIMELIGHT_NAME
+import com.team4099.robot2023.util.LimelightReading
+import com.team4099.utils.LimelightHelpers
+import edu.wpi.first.networktables.NetworkTableEntry
+import edu.wpi.first.networktables.NetworkTableInstance
+import org.team4099.lib.units.base.inMilliseconds
+import org.team4099.lib.units.base.seconds
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.milli
+
+class LimelightVisionIOReal : LimelightVisionIO {
+
+  private val ledEntry: NetworkTableEntry =
+    NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("ledMode")
+  private val pipelineEntry: NetworkTableEntry =
+    NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("pipeline")
+  private val validEntry: NetworkTableEntry =
+    NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("tv")
+  private val latencyEntry: NetworkTableEntry =
+    NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("tl")
+  private val captureLatencyEntry: NetworkTableEntry =
+    NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("cl")
+  private val dataEntry: NetworkTableEntry =
+    NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("tcornxy")
+  private val angleEntry: NetworkTableEntry =
+    NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("tx")
+
+  override fun updateInputs(inputs: LimelightVisionIO.LimelightVisionIOInputs) {
+    val totalLatency =
+      (
+        latencyEntry.getDouble(0.0).milli.seconds +
+          captureLatencyEntry.getDouble(0.0).milli.seconds
+        )
+
+    inputs.timestamp = Clock.realTimestamp - totalLatency
+    inputs.angle = angleEntry.getDouble(0.0).degrees
+    inputs.fps = 1000 / totalLatency.inMilliseconds
+    inputs.validReading = LimelightHelpers.getTV(LIMELIGHT_NAME)
+
+    inputs.retroTargets =
+      LimelightHelpers.getLatestResults(LIMELIGHT_NAME).targetingResults.targets_Retro.map {
+        LimelightReading(it)
+      }
+  }
+
+  override fun setPipeline(pipelineIndex: Int) {
+    pipelineEntry.setInteger(pipelineIndex.toLong())
+  }
+
+  override fun setLeds(enabled: Boolean) {
+    ledEntry.setBoolean(enabled)
+  }
+}
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
index bd88ac4e..d5bfee73 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
@@ -328,8 +328,6 @@ class Superstructure(
                 }
               }
             }
-            is SuperstructureRequest.SingleSubstationIntakePrep ->
-              SuperstructureStates.SINGLE_SUBSTATION_INTAKE_PREP
             is SuperstructureRequest.Tuning -> SuperstructureStates.TUNING
             else -> currentState
           }
@@ -728,7 +726,8 @@ class Superstructure(
 
         groundIntake.currentRequest =
           Request.GroundIntakeRequest.TargetingPosition(
-            GroundIntake.TunableGroundIntakeStates.stowedDownAngle.get(), 0.0.volts
+            GroundIntake.TunableGroundIntakeStates.stowedDownAngle.get(),
+            GroundIntake.TunableGroundIntakeStates.outtakeVoltage.get()
           )
 
         if (groundIntake.isAtTargetedPosition || groundIntake.canContinueSafely) {
@@ -748,9 +747,7 @@ class Superstructure(
             when (nodeTier) {
               NodeTier.HYBRID -> {
                 when (usingGamePiece) {
-                  GamePiece.CUBE ->
-                    Elevator.TunableElevatorHeights.hybridHeight.get() +
-                      Elevator.TunableElevatorHeights.cubeDropPosition.get()
+                  GamePiece.CUBE -> Elevator.TunableElevatorHeights.groundIntakeCubeHeight.get()
                   GamePiece.CONE ->
                     Elevator.TunableElevatorHeights.hybridHeight.get() +
                       Elevator.TunableElevatorHeights.coneDropPosition.get()
diff --git a/src/main/kotlin/com/team4099/robot2023/util/FieldGeomUtil.kt b/src/main/kotlin/com/team4099/robot2023/util/FieldGeomUtil.kt
index 8d579d25..e69924c1 100644
--- a/src/main/kotlin/com/team4099/robot2023/util/FieldGeomUtil.kt
+++ b/src/main/kotlin/com/team4099/robot2023/util/FieldGeomUtil.kt
@@ -2,6 +2,11 @@ package com.team4099.robot2023.util
 
 import com.team4099.robot2023.config.constants.FieldConstants
 import org.team4099.lib.geometry.Pose2d
+import org.team4099.lib.geometry.Pose3d
+import org.team4099.lib.geometry.Transform3d
+import org.team4099.lib.units.base.inMeters
+import kotlin.math.pow
+import kotlin.math.sqrt
 
 fun Pose2d.isOnInnerSideOfChargeStation(): Boolean {
   val normalizedPose = AllianceFlipUtil.apply(this)
@@ -18,3 +23,44 @@ fun Pose2d.isAboveMiddleOfChargeStation(): Boolean {
         ) / 2
       )
 }
+
+fun Pose3d.findClosestPose(vararg pose3d: Pose3d): Pose3d {
+  var closestPose = pose3d[0]
+  if (pose3d.size > 1) {
+    for (pose in pose3d) {
+      if (this.closerToInTranslation(pose, closestPose) == pose) {
+        closestPose = pose
+      }
+    }
+  }
+
+  return closestPose
+}
+
+fun Transform3d.toPose3d(): Pose3d{
+  return Pose3d(this.translation, this.rotation)
+}
+
+fun Pose3d.toTransform3d(): Transform3d{
+  return Transform3d(this.translation, this.rotation)
+}
+
+fun Pose3d.closerToInTranslation(pose1: Pose3d, pose2: Pose3d): Pose3d {
+  val distToPose1 =
+    sqrt(
+      (this.x - pose1.x).inMeters.pow(2) +
+        (this.y - pose1.y).inMeters.pow(2) +
+        (this.z - pose1.z).inMeters.pow(2)
+    )
+  val distToPose2 =
+    sqrt(
+      (this.x - pose2.x).inMeters.pow(2) +
+        (this.y - pose2.y).inMeters.pow(2) +
+        (this.z - pose2.z).inMeters.pow(2)
+    )
+  if (distToPose1 < distToPose2) {
+    return pose1
+  } else {
+    return pose2
+  }
+}
diff --git a/src/main/kotlin/com/team4099/robot2023/util/GeomUtil.kt b/src/main/kotlin/com/team4099/robot2023/util/GeomUtil.kt
index 1e942eb1..fcca9ced 100644
--- a/src/main/kotlin/com/team4099/robot2023/util/GeomUtil.kt
+++ b/src/main/kotlin/com/team4099/robot2023/util/GeomUtil.kt
@@ -1,6 +1,14 @@
 package com.team4099.robot2023.util
 
+import org.team4099.lib.geometry.Pose2d
+import org.team4099.lib.geometry.Pose3d
+import org.team4099.lib.geometry.Rotation3d
 import org.team4099.lib.geometry.Twist2d
+import org.team4099.lib.units.base.meters
+import org.team4099.lib.units.derived.Angle
+import org.team4099.lib.units.derived.angle
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inRotation2ds
 
 /**
  * Multiplies a twist by a scaling factor
@@ -12,3 +20,11 @@ import org.team4099.lib.geometry.Twist2d
 fun multiplyTwist(twist: Twist2d, factor: Double): Twist2d {
   return Twist2d(twist.dx * factor, twist.dy * factor, twist.dtheta * factor)
 }
+
+fun Pose2d.toPose3d(): Pose3d {
+  return Pose3d(this.x, this.y, 0.0.meters, Rotation3d(0.0.degrees, 0.0.degrees, this.rotation))
+}
+
+fun Angle.rotateBy(angle: Angle): Angle {
+  return this.inRotation2ds.rotateBy(angle.inRotation2ds).angle
+}
diff --git a/src/main/kotlin/com/team4099/robot2023/util/LimelightReading.kt b/src/main/kotlin/com/team4099/robot2023/util/LimelightReading.kt
new file mode 100644
index 00000000..388dc94e
--- /dev/null
+++ b/src/main/kotlin/com/team4099/robot2023/util/LimelightReading.kt
@@ -0,0 +1,24 @@
+package com.team4099.robot2023.util
+
+import com.team4099.utils.LimelightHelpers.LimelightTarget_Retro
+import org.team4099.lib.units.derived.Angle
+import org.team4099.lib.units.derived.degrees
+
+data class LimelightReading(
+  val tx: Angle,
+  val ty: Angle,
+  val txPixel: Double,
+  val tyPixel: Double,
+  val ts: Angle
+) {
+
+  constructor(
+    limelightReading: LimelightTarget_Retro
+  ) : this(
+    limelightReading.tx.degrees,
+    limelightReading.ty.degrees,
+    limelightReading.tx_pixels,
+    limelightReading.ty_pixels,
+    limelightReading.ts.degrees
+  ) {}
+}