From 95731918c762c2b1d60daf5edd4d2c021095ac06 Mon Sep 17 00:00:00 2001
From: Isaac <54126919+Aizakkuno@users.noreply.github.com>
Date: Fri, 22 Mar 2024 18:45:49 -0400
Subject: [PATCH 01/23] adapted both example code segments together for pose
 estimation with custom constants into drivetrain update odometry func

---
 src/main/java/frc/robot/LimelightHelpers.java | 949 ++++++++++++++++++
 .../java/frc/robot/Subsystems/Drivetrain.java |  68 ++
 2 files changed, 1017 insertions(+)
 create mode 100644 src/main/java/frc/robot/LimelightHelpers.java

diff --git a/src/main/java/frc/robot/LimelightHelpers.java b/src/main/java/frc/robot/LimelightHelpers.java
new file mode 100644
index 0000000..4d5c6d3
--- /dev/null
+++ b/src/main/java/frc/robot/LimelightHelpers.java
@@ -0,0 +1,949 @@
+//LimelightHelpers v1.4.0 (March 21, 2024)
+
+package frc.robot;
+
+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("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 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 String error;
+
+        public LimelightResults() {
+            targetingResults = new Results();
+            error = "";
+        }
+
+
+    }
+
+    public static class RawFiducial {
+        public int id;
+        public double txnc;
+        public double tync;
+        public double ta;
+        public double distToCamera;
+        public double distToRobot;
+        public double ambiguity;
+
+
+        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;
+        }
+    }
+
+    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 PoseEstimate(Pose2d pose, double timestampSeconds, double latency, 
+            int tagCount, double tagSpan, double avgTagDist, 
+            double avgTagArea, RawFiducial[] rawFiducials) {
+
+            this.pose = pose;
+            this.timestampSeconds = timestampSeconds;
+            this.latency = latency;
+            this.tagCount = tagCount;
+            this.tagSpan = tagSpan;
+            this.avgTagDist = avgTagDist;
+            this.avgTagArea = avgTagArea;
+            this.rawFiducials = rawFiducials;
+        }
+    }
+
+    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);
+    }
+
+    private static double extractBotPoseEntry(double[] inData, int position){
+        if(inData.length < position+1)
+        {
+            return 0;
+        }
+        return inData[position];
+    }
+
+    private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) {
+        var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName);
+        var poseArray = poseEntry.getDoubleArray(new double[0]);
+        var pose = toPose2D(poseArray);
+        double latency = extractBotPoseEntry(poseArray,6);
+        int tagCount = (int)extractBotPoseEntry(poseArray,7);
+        double tagSpan = extractBotPoseEntry(poseArray,8);
+        double tagDist = extractBotPoseEntry(poseArray,9);
+        double tagArea = extractBotPoseEntry(poseArray,10);
+        //getlastchange() in microseconds, ll latency in milliseconds
+        var timestamp = (poseEntry.getLastChange() / 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, timestamp,latency,tagCount,tagSpan,tagDist,tagArea,rawFiducials);
+    }
+
+    private 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.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 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 String getNeuralClassID(String limelightName) {
+        return getLimelightNTString(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 and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE
+     * alliance
+     * 
+     * @param limelightName
+     * @return
+     */
+    public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) {
+        return getBotPoseEstimate(limelightName, "botpose_wpiblue");
+    }
+
+    /**
+     * 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");
+    }
+
+    /**
+     * 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);
+    }
+
+    
+    public static void setPriorityTagID(String limelightName, int ID) {
+        setLimelightNTDouble(limelightName, "priorityid", ID);
+    }
+
+    /**
+     * 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) {
+            results.error = "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;
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Subsystems/Drivetrain.java b/src/main/java/frc/robot/Subsystems/Drivetrain.java
index 3c255ff..38afa7d 100644
--- a/src/main/java/frc/robot/Subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/Subsystems/Drivetrain.java
@@ -6,6 +6,7 @@
 import com.swervedrivespecialties.swervelib.SdsModuleConfigurations;
 import com.swervedrivespecialties.swervelib.SwerveModule;
 
+import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -13,6 +14,7 @@
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
@@ -21,7 +23,9 @@
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.LimelightHelpers;
 import frc.robot.Constants.Constants;
+import frc.robot.LimelightHelpers.PoseEstimate;
 
 public class Drivetrain extends Subsystem{
     private static Drivetrain _instance;
@@ -45,6 +49,27 @@ public class Drivetrain extends Subsystem{
 
     private final Field2d m_field = new Field2d();
 
+    private final String kLimelightName = "";
+
+    // in meters
+    private final double kNearTagAreaMin = 0.8;
+    private final double kFarTagAreaMin = 0.1;
+
+    private final double kMidTrustPoseDiffMax = 0.5;
+    private final double kLowTrustPoseDiffMax = 0.3;
+
+    // std = standard deviation
+    // in meters
+    private final double kHighTrustXYStds = 0.5;
+    private final double kMidTrustXYStds = 1.0;
+    private final double kLowTrustXYStds = 2.0;
+
+    // rotation deviation
+    // in degrees
+    private final double kHighTrustDegStds = 6;
+    private final double kMidTrustDegStds = 12;
+    private final double kLowTrustDegStds = 30;
+
     private Drivetrain()
     {
         ShuffleboardTab tab = Shuffleboard.getTab("Drivetrain");
@@ -168,6 +193,49 @@ public void updateOdometry()
                 _backRightModule.getPosition()
             }
         );
+
+        PoseEstimate poseEstimate = LimelightHelpers
+            .getBotPoseEstimate_wpiBlue(kLimelightName);
+
+        if (poseEstimate.tagCount == 0) return;
+
+        double poseDifference = _drivePoseEstimator
+            .getEstimatedPosition()
+            .getTranslation()
+            .getDistance(poseEstimate.pose.getTranslation());
+
+        double xyStds;
+        double degStds;
+        
+        // note that besides the tag count check, there are no equals checks 
+        // (e.g. <=) just < and >, for var name consistency we use =
+        if (poseEstimate.tagCount >= 2) {
+            xyStds = kHighTrustXYStds;
+            degStds = kHighTrustDegStds;
+
+        } else if (poseEstimate.avgTagArea >= kNearTagAreaMin &&
+                   poseDifference <= kMidTrustPoseDiffMax) {
+
+            xyStds = kMidTrustXYStds;
+            degStds = kMidTrustDegStds;
+
+        } else if (poseEstimate.avgTagArea >= kFarTagAreaMin &&
+                   poseDifference <= kLowTrustPoseDiffMax) {
+
+            xyStds = kLowTrustXYStds;
+            degStds = kLowTrustDegStds;
+        } else {
+            return;
+        }
+
+        _drivePoseEstimator.setVisionMeasurementStdDevs(
+            VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds))
+        );
+
+        _drivePoseEstimator.addVisionMeasurement(
+            poseEstimate.pose,
+            Timer.getFPGATimestamp() - poseEstimate.latency
+        );
     }
 
     // public void resetOdometry()

From cf6e2fb31eba01e9f60df95253a0f2345253015e Mon Sep 17 00:00:00 2001
From: Isaac <54126919+Aizakkuno@users.noreply.github.com>
Date: Sat, 23 Mar 2024 10:33:49 -0400
Subject: [PATCH 02/23] Update Drivetrain.java

---
 src/main/java/frc/robot/Subsystems/Drivetrain.java | 13 +++++++++----
 1 file changed, 9 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/Subsystems/Drivetrain.java b/src/main/java/frc/robot/Subsystems/Drivetrain.java
index 38afa7d..d90691b 100644
--- a/src/main/java/frc/robot/Subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/Subsystems/Drivetrain.java
@@ -36,6 +36,7 @@ public class Drivetrain extends Subsystem{
     private final SwerveModule _backRightModule;
 
     private final SwerveDrivePoseEstimator _drivePoseEstimator;
+    private final SwerveDrivePoseEstimator _visionPoseEstimator;
 
     private SwerveModuleState[] _moduleStates = new SwerveModuleState[4];
 
@@ -48,6 +49,7 @@ public class Drivetrain extends Subsystem{
     private ChassisSpeeds _chassisSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0);
 
     private final Field2d m_field = new Field2d();
+    private final Field2d m_vPEField = new Field2d();
 
     private final String kLimelightName = "";
 
@@ -133,7 +135,8 @@ private Drivetrain()
             },
             new Pose2d()
         );
-        SmartDashboard.putData("Field",m_field);
+
+        SmartDashboard.putData("Field", m_field);
     }
 
     public static Drivetrain getInstance()
@@ -156,6 +159,8 @@ public void logTelemetry()
         
         m_field.setRobotPose(pose);
 
+        m_vPEField.setRobotPose(_visionPoseEstimator.getEstimatedPosition());
+
         SmartDashboard.putNumber("drivetrainGyroAngle",
                                  getGyroRotation().getDegrees());
         SmartDashboard.putNumber("drivetrainGyroAngleWithOffset",
@@ -199,7 +204,7 @@ public void updateOdometry()
 
         if (poseEstimate.tagCount == 0) return;
 
-        double poseDifference = _drivePoseEstimator
+        double poseDifference = _visionPoseEstimator
             .getEstimatedPosition()
             .getTranslation()
             .getDistance(poseEstimate.pose.getTranslation());
@@ -228,11 +233,11 @@ public void updateOdometry()
             return;
         }
 
-        _drivePoseEstimator.setVisionMeasurementStdDevs(
+        _visionPoseEstimator.setVisionMeasurementStdDevs(
             VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds))
         );
 
-        _drivePoseEstimator.addVisionMeasurement(
+        _visionPoseEstimator.addVisionMeasurement(
             poseEstimate.pose,
             Timer.getFPGATimestamp() - poseEstimate.latency
         );

From 759bc3f941136dea244fdb4a6c18308c69fa026d Mon Sep 17 00:00:00 2001
From: Isaac <54126919+Aizakkuno@users.noreply.github.com>
Date: Sat, 23 Mar 2024 20:25:00 -0400
Subject: [PATCH 03/23] added new field for vision estimate

---
 src/main/java/frc/robot/Subsystems/Drivetrain.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/Subsystems/Drivetrain.java b/src/main/java/frc/robot/Subsystems/Drivetrain.java
index d90691b..fbbfb72 100644
--- a/src/main/java/frc/robot/Subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/Subsystems/Drivetrain.java
@@ -137,6 +137,7 @@ private Drivetrain()
         );
 
         SmartDashboard.putData("Field", m_field);
+        SmartDashboard.putData("Vision Estimate Field", m_vPEField);
     }
 
     public static Drivetrain getInstance()

From ddf567f7876c56b4f3d3d8d32dc2ad92b14b7989 Mon Sep 17 00:00:00 2001
From: SCPhillips06 <128173664+SCPhillips06@users.noreply.github.com>
Date: Mon, 25 Mar 2024 19:08:32 -0400
Subject: [PATCH 04/23] set to main pose estimator

---
 src/main/java/frc/robot/Subsystems/Drivetrain.java | 12 +++++++-----
 1 file changed, 7 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/Subsystems/Drivetrain.java b/src/main/java/frc/robot/Subsystems/Drivetrain.java
index fbbfb72..289695e 100644
--- a/src/main/java/frc/robot/Subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/Subsystems/Drivetrain.java
@@ -137,7 +137,7 @@ private Drivetrain()
         );
 
         SmartDashboard.putData("Field", m_field);
-        SmartDashboard.putData("Vision Estimate Field", m_vPEField);
+        
     }
 
     public static Drivetrain getInstance()
@@ -160,7 +160,7 @@ public void logTelemetry()
         
         m_field.setRobotPose(pose);
 
-        m_vPEField.setRobotPose(_visionPoseEstimator.getEstimatedPosition());
+        // m_vPEField.setRobotPose(_visionPoseEstimator.getEstimatedPosition());
 
         SmartDashboard.putNumber("drivetrainGyroAngle",
                                  getGyroRotation().getDegrees());
@@ -205,7 +205,9 @@ public void updateOdometry()
 
         if (poseEstimate.tagCount == 0) return;
 
-        double poseDifference = _visionPoseEstimator
+        SmartDashboard.putNumber("Vision Ambiguity", poseEstimate.rawFiducials[0].ambiguity);
+
+        double poseDifference = _drivePoseEstimator
             .getEstimatedPosition()
             .getTranslation()
             .getDistance(poseEstimate.pose.getTranslation());
@@ -234,11 +236,11 @@ public void updateOdometry()
             return;
         }
 
-        _visionPoseEstimator.setVisionMeasurementStdDevs(
+        _drivePoseEstimator.setVisionMeasurementStdDevs(
             VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds))
         );
 
-        _visionPoseEstimator.addVisionMeasurement(
+        _drivePoseEstimator.addVisionMeasurement(
             poseEstimate.pose,
             Timer.getFPGATimestamp() - poseEstimate.latency
         );

From ab510156d22da2cedaa0b1aa0f5b963fd01027a5 Mon Sep 17 00:00:00 2001
From: Isaac <54126919+Aizakkuno@users.noreply.github.com>
Date: Mon, 25 Mar 2024 19:09:45 -0400
Subject: [PATCH 05/23] added constants

---
 src/main/java/frc/robot/Autonomous/AutoCmds.java   | 13 +++++++++++++
 src/main/java/frc/robot/Constants/Constants.java   |  4 ++++
 src/main/java/frc/robot/Subsystems/Drivetrain.java | 14 ++++++++++++--
 src/main/java/frc/robot/Subsystems/Jukebox.java    |  1 +
 4 files changed, 30 insertions(+), 2 deletions(-)
 create mode 100644 src/main/java/frc/robot/Autonomous/AutoCmds.java

diff --git a/src/main/java/frc/robot/Autonomous/AutoCmds.java b/src/main/java/frc/robot/Autonomous/AutoCmds.java
new file mode 100644
index 0000000..a4bafa9
--- /dev/null
+++ b/src/main/java/frc/robot/Autonomous/AutoCmds.java
@@ -0,0 +1,13 @@
+package frc.robot.Autonomous;
+
+public enum AutoCmds {
+    FOLLOW_WITH_POSE_AIM,
+    FOLLOW_WITH_SHOOT_AIM,
+    FOLLOW_WITH_FULL_AIM,
+    FOLLOW_WITH_PASSTHROUGH,
+    FOLLOW,
+    SHOOT,
+    AIM,
+    POSE_AIM,
+    SHOOT_AIM
+}
diff --git a/src/main/java/frc/robot/Constants/Constants.java b/src/main/java/frc/robot/Constants/Constants.java
index 5a1d41f..3892a5b 100644
--- a/src/main/java/frc/robot/Constants/Constants.java
+++ b/src/main/java/frc/robot/Constants/Constants.java
@@ -6,6 +6,10 @@
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 
 public final class Constants {
+    public static final double kFieldLength = 16.54;
+    public static final double kFieldWidth = 8.21;
+
+    // public static final double k
 
     // Controller USB ports
     public static final int kDriverControllerUsbSlot = 0;
diff --git a/src/main/java/frc/robot/Subsystems/Drivetrain.java b/src/main/java/frc/robot/Subsystems/Drivetrain.java
index d90691b..8f45cf2 100644
--- a/src/main/java/frc/robot/Subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/Subsystems/Drivetrain.java
@@ -10,6 +10,7 @@
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -72,6 +73,10 @@ public class Drivetrain extends Subsystem{
     private final double kMidTrustDegStds = 12;
     private final double kLowTrustDegStds = 30;
 
+    // TODO: get these values
+    private final Translation2d kSpeakerTargetBlue = new Translation2d(0.5, 0.5);
+    private final Translation2d kSpeakerTargetRed = new Translation2d(0.5, 0.5);
+
     private Drivetrain()
     {
         ShuffleboardTab tab = Shuffleboard.getTab("Drivetrain");
@@ -313,10 +318,11 @@ public SwerveModulePosition[] getModulePositions()
      */ 
     public void fieldOrientedDrive(double translationX,
                                    double translationY,
-                                   double rotationZ)
-    {   
+                                   double rotationZ) {   
+
         var alliance = DriverStation.getAlliance();
         var invert = alliance.isPresent() && alliance.get() == Alliance.Red ? -1 : 1; 
+
         _chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
             invert * translationX * Constants.MAX_VELOCITY_METERS_PER_SECOND,
             invert * translationY * Constants.MAX_VELOCITY_METERS_PER_SECOND, 
@@ -354,6 +360,10 @@ public void drive(ChassisSpeeds chassisSpeeds)
         _chassisSpeeds = chassisSpeeds;
     }
 
+    public void getTargetAngle() {
+        Pose2d targetPose
+    }
+
     /** 
      * Method that resets the pigeon current direction the robot is facing will be the front
      */
diff --git a/src/main/java/frc/robot/Subsystems/Jukebox.java b/src/main/java/frc/robot/Subsystems/Jukebox.java
index 49fb309..b84c04a 100644
--- a/src/main/java/frc/robot/Subsystems/Jukebox.java
+++ b/src/main/java/frc/robot/Subsystems/Jukebox.java
@@ -550,6 +550,7 @@ private void prepSpeaker() {
                 kShooterSpeeds,
                 _targetDistance
             );
+
             setShooterAngle(desiredShooterAngle);
             setShooterSpeed(desiredShooterSpeed);
 

From 57f7f64fc009bbae0ae79c52fb8a68c95b3ba209 Mon Sep 17 00:00:00 2001
From: Isaac <54126919+Aizakkuno@users.noreply.github.com>
Date: Wed, 27 Mar 2024 18:21:59 -0400
Subject: [PATCH 06/23] new stuff

---
 .../java/frc/robot/Subsystems/Drivetrain.java | 36 ++++++++++++++-----
 1 file changed, 28 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/Subsystems/Drivetrain.java b/src/main/java/frc/robot/Subsystems/Drivetrain.java
index c8b7f97..2bf3c1f 100644
--- a/src/main/java/frc/robot/Subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/Subsystems/Drivetrain.java
@@ -27,6 +27,7 @@
 import frc.robot.LimelightHelpers;
 import frc.robot.Constants.Constants;
 import frc.robot.LimelightHelpers.PoseEstimate;
+import frc.robot.LimelightHelpers.RawFiducial;
 
 public class Drivetrain extends Subsystem{
     private static Drivetrain _instance;
@@ -37,7 +38,6 @@ public class Drivetrain extends Subsystem{
     private final SwerveModule _backRightModule;
 
     private final SwerveDrivePoseEstimator _drivePoseEstimator;
-    private final SwerveDrivePoseEstimator _visionPoseEstimator;
 
     private SwerveModuleState[] _moduleStates = new SwerveModuleState[4];
 
@@ -210,13 +210,26 @@ public void updateOdometry()
 
         if (poseEstimate.tagCount == 0) return;
 
-        SmartDashboard.putNumber("Vision Ambiguity", poseEstimate.rawFiducials[0].ambiguity);
+        if (poseEstimate.tagCount == 1 &&
+            poseEstimate.rawFiducials[0].ambiguity > 0.5) {
+
+            return;
+        }
 
         double poseDifference = _drivePoseEstimator
             .getEstimatedPosition()
             .getTranslation()
             .getDistance(poseEstimate.pose.getTranslation());
 
+        double bestTagArea = 0;
+        for (int i = 0; i < poseEstimate.rawFiducials.length; i++) {
+            RawFiducial rawFiducial = poseEstimate.rawFiducials[i];
+
+            if (rawFiducial.ta > bestTagArea) {
+                bestTagArea = rawFiducial.ta;
+            }
+        }
+
         double xyStds;
         double degStds;
         
@@ -226,13 +239,13 @@ public void updateOdometry()
             xyStds = kHighTrustXYStds;
             degStds = kHighTrustDegStds;
 
-        } else if (poseEstimate.avgTagArea >= kNearTagAreaMin &&
+        } else if (bestTagArea >= kNearTagAreaMin &&
                    poseDifference <= kMidTrustPoseDiffMax) {
 
             xyStds = kMidTrustXYStds;
             degStds = kMidTrustDegStds;
 
-        } else if (poseEstimate.avgTagArea >= kFarTagAreaMin &&
+        } else if (bestTagArea >= kFarTagAreaMin &&
                    poseDifference <= kLowTrustPoseDiffMax) {
 
             xyStds = kLowTrustXYStds;
@@ -242,11 +255,18 @@ public void updateOdometry()
         }
 
         _drivePoseEstimator.setVisionMeasurementStdDevs(
+            // VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds))
             VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds))
         );
 
+        Pose2d pose = new Pose2d(
+            poseEstimate.pose.getX(),
+            poseEstimate.pose.getY(),
+            getGyroRotation()
+        );
+
         _drivePoseEstimator.addVisionMeasurement(
-            poseEstimate.pose,
+            pose,
             Timer.getFPGATimestamp() - poseEstimate.latency
         );
     }
@@ -363,9 +383,9 @@ public void drive(ChassisSpeeds chassisSpeeds)
         _chassisSpeeds = chassisSpeeds;
     }
 
-    public void getTargetAngle() {
-        Pose2d targetPose
-    }
+    // public void getTargetAngle() {
+    //     Pose2d targetPose
+    // }
 
     /** 
      * Method that resets the pigeon current direction the robot is facing will be the front

From 4aa4d55aee2300c3d16556e2def6df4e30235c62 Mon Sep 17 00:00:00 2001
From: Isaac <54126919+Aizakkuno@users.noreply.github.com>
Date: Mon, 1 Apr 2024 18:13:13 -0400
Subject: [PATCH 07/23] testing changes

---
 .../java/frc/robot/Subsystems/Drivetrain.java | 20 +++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/Subsystems/Drivetrain.java b/src/main/java/frc/robot/Subsystems/Drivetrain.java
index 2bf3c1f..ccd8295 100644
--- a/src/main/java/frc/robot/Subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/Subsystems/Drivetrain.java
@@ -254,19 +254,19 @@ public void updateOdometry()
             return;
         }
 
-        _drivePoseEstimator.setVisionMeasurementStdDevs(
-            // VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds))
-            VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds))
-        );
+        // _drivePoseEstimator.setVisionMeasurementStdDevs(
+        //     // VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds))
+        //     VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds))
+        // );
 
-        Pose2d pose = new Pose2d(
-            poseEstimate.pose.getX(),
-            poseEstimate.pose.getY(),
-            getGyroRotation()
-        );
+        // Pose2d pose = new Pose2d(
+        //     poseEstimate.pose.getX(),
+        //     poseEstimate.pose.getY(),
+        //     getGyroRotation()
+        // );
 
         _drivePoseEstimator.addVisionMeasurement(
-            pose,
+            poseEstimate.pose,
             Timer.getFPGATimestamp() - poseEstimate.latency
         );
     }

From f8f4c70c0fb51fc655c0b06c6ede86e15127d345 Mon Sep 17 00:00:00 2001
From: SCPhillips06 <128173664+SCPhillips06@users.noreply.github.com>
Date: Mon, 1 Apr 2024 20:11:33 -0400
Subject: [PATCH 08/23] estimation working

---
 src/main/java/frc/robot/Robot.java            |  2 +-
 .../java/frc/robot/Subsystems/Drivetrain.java | 27 ++++++++++++++-----
 2 files changed, 22 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 682801a..7d022a4 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -98,7 +98,7 @@ public void teleopPeriodic() {
     // _ledController.handleBottomLights(); Add this code when we get the bottom lights setup on the robot
     teleopDrive();
     teleopJukebox();
-    _drivetrain.updateOdometry();
+    _drivetrain.updateOdometryWithVision();
 
     teleopIntake();
     teleopClimb();
diff --git a/src/main/java/frc/robot/Subsystems/Drivetrain.java b/src/main/java/frc/robot/Subsystems/Drivetrain.java
index ccd8295..4aa90f2 100644
--- a/src/main/java/frc/robot/Subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/Subsystems/Drivetrain.java
@@ -190,8 +190,21 @@ public void logTelemetry()
         SmartDashboard.putNumber("drivetrainOdometryZ",
                                  pose.getRotation().getDegrees());
     }
+
+    public void updateOdometry() {
+        _drivePoseEstimator.updateWithTime(
+            Timer.getFPGATimestamp(),
+            getGyroRotation(),
+            new SwerveModulePosition[] {
+                _frontLeftModule.getPosition(),
+                _frontRightModule.getPosition(),
+                _backLeftModule.getPosition(),
+                _backRightModule.getPosition()
+            }
+        );
+    }
     
-    public void updateOdometry()
+    public void updateOdometryWithVision()
     {
 
         _drivePoseEstimator.updateWithTime(
@@ -254,10 +267,10 @@ public void updateOdometry()
             return;
         }
 
-        // _drivePoseEstimator.setVisionMeasurementStdDevs(
-        //     // VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds))
-        //     VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds))
-        // );
+        _drivePoseEstimator.setVisionMeasurementStdDevs(
+            // VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds))
+            VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds))
+        );
 
         // Pose2d pose = new Pose2d(
         //     poseEstimate.pose.getX(),
@@ -265,9 +278,11 @@ public void updateOdometry()
         //     getGyroRotation()
         // );
 
+        SmartDashboard.putNumber("latency", poseEstimate.latency);
+
         _drivePoseEstimator.addVisionMeasurement(
             poseEstimate.pose,
-            Timer.getFPGATimestamp() - poseEstimate.latency
+            Timer.getFPGATimestamp() - (poseEstimate.latency / 1000)
         );
     }
 

From 73be8a70b1cd2aff73d0a3f5ce1fe11f61d734ad Mon Sep 17 00:00:00 2001
From: Isaac <54126919+Aizakkuno@users.noreply.github.com>
Date: Tue, 2 Apr 2024 09:14:22 -0400
Subject: [PATCH 09/23] entered in some points

---
 .../java/frc/robot/Constants/Constants.java   | 10 +++++++
 src/main/java/frc/robot/Robot.java            | 29 ++++++++++++++++++-
 .../java/frc/robot/Subsystems/Jukebox.java    | 23 ++++++++++++++-
 3 files changed, 60 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/Constants/Constants.java b/src/main/java/frc/robot/Constants/Constants.java
index 8314aa2..f8c433c 100644
--- a/src/main/java/frc/robot/Constants/Constants.java
+++ b/src/main/java/frc/robot/Constants/Constants.java
@@ -9,6 +9,16 @@ public final class Constants {
     public static final double kFieldLength = 16.54;
     public static final double kFieldWidth = 8.21;
 
+    public static final Translation2d kBlueSpeaker = new Translation2d(
+        0,
+        5.5
+    );
+
+    public static final Translation2d kRedSpeaker = new Translation2d(
+        kBlueSpeaker.getX() + kFieldLength,
+        kBlueSpeaker.getY()
+    )
+
     // public static final double k
 
     // Controller USB ports
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 7d022a4..9c93741 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,9 +4,14 @@
 
 package frc.robot;
 
+import java.util.Optional;
+
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.DataLogManager;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import frc.robot.Autonomous.AutonomousMode;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -23,6 +28,7 @@
  * project.
  */
 public class Robot extends TimedRobot {
+    private static Translation2d kSpeaker;
   /**
    * This function is run when the robot is first started up and should be used for any
    * initialization code.
@@ -64,6 +70,14 @@ public void robotPeriodic() {
     _jukebox.logTelemetry();
     _intake.logTelemetry();
     _ledController.handleLights();
+
+    Optional<Alliance> alliance = DriverStation.getAlliance();
+
+    if (alliance.isPresent()) {
+      kSpeaker = alliance.get() == Alliance.Blue ?
+        Constants.kBlueSpeaker :
+        Constants.kRedSpeaker;
+    }
   }
 
   @Override
@@ -126,9 +140,22 @@ private void teleopDrive() {
       _drivetrain.reset();
     }
 
+    double angle = _drivetrain
+      .getGyroRotation()
+      .minus(kSpeaker.getAngle())
+      .getDegrees();
+
+    double distance = _drivetrain
+      .getPose()
+      .getTranslation()
+      .getDistance(kSpeaker);
+
+    SmartDashboard.putNumber("angle to speaker", angle);
+    SmartDashboard.putNumber("distance to speaker", angle);
+
     if (Math.abs(_driverController.getLeftTriggerAxis()) > 0.25)
     {
-        rotation = -(_visionSystem.getTargetAngle() / 105) ;
+        rotation = -(angle / 105);
         //target angle range is -27 to 27 degrees
     }
 
diff --git a/src/main/java/frc/robot/Subsystems/Jukebox.java b/src/main/java/frc/robot/Subsystems/Jukebox.java
index 60ae638..405b70a 100644
--- a/src/main/java/frc/robot/Subsystems/Jukebox.java
+++ b/src/main/java/frc/robot/Subsystems/Jukebox.java
@@ -8,11 +8,16 @@
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
+import java.util.Optional;
+
 import com.revrobotics.CANSparkBase;
 import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkLowLevel.MotorType;
@@ -155,6 +160,8 @@ public class Jukebox extends Subsystem{
 
     private int _loopCounter = 0;
 
+    private static Translation2d kSpeaker;
+
     public Jukebox()
     {
         // Config Elevator
@@ -567,7 +574,21 @@ private void prepTrap() {
     private void prepSpeaker() {
         setElevatorPosition(0);
 
-        _targetDistance = _visionSystem.getDistance();
+        // _targetDistance = _visionSystem.getDistance();
+
+        Optional<Alliance> alliance = DriverStation.getAlliance();
+
+        if (alliance.isPresent()) {
+            kSpeaker = alliance.get() == Alliance.Blue ?
+                Constants.kBlueSpeaker :
+                Constants.kRedSpeaker;
+        }
+
+        _targetDistance = Drivetrain
+            .getInstance()
+            .getPose()
+            .getTranslation()
+            .getDistance(kSpeaker);
 
         feeder();
 

From 1be75a8a3b2495ca478546517c2d148e9ac5221e Mon Sep 17 00:00:00 2001
From: SCPhillips06 <128173664+SCPhillips06@users.noreply.github.com>
Date: Tue, 2 Apr 2024 09:58:17 -0400
Subject: [PATCH 10/23] working on correct angle readings

---
 src/main/java/frc/robot/Constants/Constants.java | 2 +-
 src/main/java/frc/robot/Robot.java               | 8 ++++----
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/Constants/Constants.java b/src/main/java/frc/robot/Constants/Constants.java
index f8c433c..398fbe6 100644
--- a/src/main/java/frc/robot/Constants/Constants.java
+++ b/src/main/java/frc/robot/Constants/Constants.java
@@ -17,7 +17,7 @@ public final class Constants {
     public static final Translation2d kRedSpeaker = new Translation2d(
         kBlueSpeaker.getX() + kFieldLength,
         kBlueSpeaker.getY()
-    )
+    );
 
     // public static final double k
 
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 9c93741..a539658 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -140,9 +140,9 @@ private void teleopDrive() {
       _drivetrain.reset();
     }
 
-    double angle = _drivetrain
-      .getGyroRotation()
-      .minus(kSpeaker.getAngle())
+    double angle = kSpeaker
+      .minus(_drivetrain.getPose().getTranslation())
+      .getAngle()
       .getDegrees();
 
     double distance = _drivetrain
@@ -151,7 +151,7 @@ private void teleopDrive() {
       .getDistance(kSpeaker);
 
     SmartDashboard.putNumber("angle to speaker", angle);
-    SmartDashboard.putNumber("distance to speaker", angle);
+    SmartDashboard.putNumber("distance to speaker", distance);
 
     if (Math.abs(_driverController.getLeftTriggerAxis()) > 0.25)
     {

From fa2d9173268f395918f785d77f0c4dfbd5b61681 Mon Sep 17 00:00:00 2001
From: Isaac <54126919+Aizakkuno@users.noreply.github.com>
Date: Tue, 2 Apr 2024 11:46:53 -0400
Subject: [PATCH 11/23] paths

---
 .../pathplanner/autos/Bottom-C-3-4-5.auto     | 43 ++++++++++++
 .../pathplanner/autos/Mid C-B-A-1 SUB.auto    | 67 +++++++++++++++++++
 .../deploy/pathplanner/paths/TA123_T-A.path   | 52 ++++++++++++++
 3 files changed, 162 insertions(+)
 create mode 100644 src/main/deploy/pathplanner/autos/Bottom-C-3-4-5.auto
 create mode 100644 src/main/deploy/pathplanner/autos/Mid C-B-A-1 SUB.auto
 create mode 100644 src/main/deploy/pathplanner/paths/TA123_T-A.path

diff --git a/src/main/deploy/pathplanner/autos/Bottom-C-3-4-5.auto b/src/main/deploy/pathplanner/autos/Bottom-C-3-4-5.auto
new file mode 100644
index 0000000..5f4ea17
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Bottom-C-3-4-5.auto
@@ -0,0 +1,43 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.7853670538059491,
+      "y": 4.459770703191282
+    },
+    "rotation": -61.144338780283555
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Bottom-C"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "C-Line"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Line-3"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "3-Line"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Mid C-B-A-1 SUB.auto b/src/main/deploy/pathplanner/autos/Mid C-B-A-1 SUB.auto
new file mode 100644
index 0000000..b686a52
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Mid C-B-A-1 SUB.auto	
@@ -0,0 +1,67 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.27,
+      "y": 5.53
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Mid C-B-A-3 SUB - Note C"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Mid C-B-A-3 SUB - Score C"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Mid C-B-A-3 SUB - Note B"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Mid C-B-A-3 SUB - Score B"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Mid C-B-A-3 SUB - Note A"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Mid C-B-A-3 SUB - Score A"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Mid C-B-A-1 SUB - Note 1"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Mid C-B-A-1 SUB - Score 1"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/TA123_T-A.path b/src/main/deploy/pathplanner/paths/TA123_T-A.path
new file mode 100644
index 0000000..6489d6a
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/TA123_T-A.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.7426336086361384,
+        "y": 6.641447399146282
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.6775001047036502,
+        "y": 6.359039811792554
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.8947741881248894,
+        "y": 6.9920223351715975
+      },
+      "prevControl": {
+        "x": 1.9501694993900078,
+        "y": 6.485636316468361
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.5,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 30.529705899934207,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "TA123",
+  "previewStartingState": {
+    "rotation": 59.47029410006579,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file

From bb89ca44750514811305dfa0c35bc0fa3af5033b Mon Sep 17 00:00:00 2001
From: Isaac <54126919+Aizakkuno@users.noreply.github.com>
Date: Tue, 2 Apr 2024 11:52:21 -0400
Subject: [PATCH 12/23] added gyro to angle

---
 src/main/java/frc/robot/Robot.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index a539658..12e415c 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -143,6 +143,7 @@ private void teleopDrive() {
     double angle = kSpeaker
       .minus(_drivetrain.getPose().getTranslation())
       .getAngle()
+      .plus(_drivetrain.getGyroRotation())
       .getDegrees();
 
     double distance = _drivetrain

From 6dd2a6263367148c45e53a034440ed5129525aaf Mon Sep 17 00:00:00 2001
From: SCPhillips06 <128173664+SCPhillips06@users.noreply.github.com>
Date: Tue, 2 Apr 2024 17:03:08 -0400
Subject: [PATCH 13/23] fixed rot aim code

---
 src/main/java/frc/robot/Robot.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 12e415c..4a9ab41 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -143,7 +143,7 @@ private void teleopDrive() {
     double angle = kSpeaker
       .minus(_drivetrain.getPose().getTranslation())
       .getAngle()
-      .plus(_drivetrain.getGyroRotation())
+      .minus(_drivetrain.getGyroRotation())
       .getDegrees();
 
     double distance = _drivetrain
@@ -156,7 +156,7 @@ private void teleopDrive() {
 
     if (Math.abs(_driverController.getLeftTriggerAxis()) > 0.25)
     {
-        rotation = -(angle / 105);
+        rotation = angle / 105;
         //target angle range is -27 to 27 degrees
     }
 

From 14c95daee7f72911bd60a8579aa708e458bc6727 Mon Sep 17 00:00:00 2001
From: SCPhillips06 <128173664+SCPhillips06@users.noreply.github.com>
Date: Tue, 2 Apr 2024 18:38:22 -0400
Subject: [PATCH 14/23] aiming and stuff working

---
 src/main/java/frc/robot/Robot.java            | 23 +++++++++----------
 .../java/frc/robot/Subsystems/Drivetrain.java | 16 +++++++++++++
 .../frc/robot/utilities/AllianceSpecific.java | 23 +++++++++++++++++++
 3 files changed, 50 insertions(+), 12 deletions(-)
 create mode 100644 src/main/java/frc/robot/utilities/AllianceSpecific.java

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 4a9ab41..0688145 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -18,6 +18,7 @@
 import frc.robot.Constants.Constants;
 import frc.robot.Enums.*;
 import frc.robot.Subsystems.*;
+import frc.robot.Utilities.AllianceSpecific;
 import frc.robot.Utilities.AutoUtil;
 import frc.robot.Utilities.LEDController;
 import frc.robot.Utilities.OneDimensionalLookup;
@@ -140,23 +141,21 @@ private void teleopDrive() {
       _drivetrain.reset();
     }
 
-    double angle = kSpeaker
-      .minus(_drivetrain.getPose().getTranslation())
-      .getAngle()
-      .minus(_drivetrain.getGyroRotation())
-      .getDegrees();
+    double speakerDistance = _drivetrain.getDistanceToTarget(
+      AllianceSpecific.getSpeaker()
+    );
 
-    double distance = _drivetrain
-      .getPose()
-      .getTranslation()
-      .getDistance(kSpeaker);
+    double speakerAngle = _drivetrain.getAngleToTarget(
+      AllianceSpecific.getSpeaker()
+    );
 
-    SmartDashboard.putNumber("angle to speaker", angle);
-    SmartDashboard.putNumber("distance to speaker", distance);
+    SmartDashboard.putNumber("angle to speaker", speakerAngle);
+    SmartDashboard.putNumber("distance to speaker", speakerDistance);
 
     if (Math.abs(_driverController.getLeftTriggerAxis()) > 0.25)
     {
-        rotation = angle / 105;
+
+        rotation = speakerAngle / 105;
         //target angle range is -27 to 27 degrees
     }
 
diff --git a/src/main/java/frc/robot/Subsystems/Drivetrain.java b/src/main/java/frc/robot/Subsystems/Drivetrain.java
index 4aa90f2..244779e 100644
--- a/src/main/java/frc/robot/Subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/Subsystems/Drivetrain.java
@@ -28,6 +28,7 @@
 import frc.robot.Constants.Constants;
 import frc.robot.LimelightHelpers.PoseEstimate;
 import frc.robot.LimelightHelpers.RawFiducial;
+import frc.robot.Utilities.AllianceSpecific;
 
 public class Drivetrain extends Subsystem{
     private static Drivetrain _instance;
@@ -419,4 +420,19 @@ public void setStartingPose(Pose2d startingPose) {
     public Pose2d getPose(){
         return _drivePoseEstimator.getEstimatedPosition();
     }
+
+    public double getAngleToTarget(Translation2d target) {
+        return target
+            .minus(getPose().getTranslation())
+            .getAngle()
+            .plus(AllianceSpecific.getAllianceAngleOffset())
+            .minus(getGyroRotationWithOffset())
+            .getDegrees();
+    }
+
+    public double getDistanceToTarget(Translation2d target) {
+        return getPose()
+            .getTranslation()
+            .getDistance(target);
+    }
 }
diff --git a/src/main/java/frc/robot/utilities/AllianceSpecific.java b/src/main/java/frc/robot/utilities/AllianceSpecific.java
new file mode 100644
index 0000000..ca65b56
--- /dev/null
+++ b/src/main/java/frc/robot/utilities/AllianceSpecific.java
@@ -0,0 +1,23 @@
+package frc.robot.Utilities;
+
+import java.util.Optional;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import frc.robot.Constants.Constants;
+
+public class AllianceSpecific {
+    private static boolean isRed() {
+        return DriverStation.getAlliance().get() == Alliance.Red;
+    }
+
+    public static Translation2d getSpeaker() {
+        return isRed() ? Constants.kRedSpeaker : Constants.kBlueSpeaker;
+    }
+
+    public static Rotation2d getAllianceAngleOffset() {
+        return Rotation2d.fromDegrees(isRed() ? 180 : 0);
+    }
+}

From 7907da6de7650a18357e45fca24e36f871463682 Mon Sep 17 00:00:00 2001
From: SCPhillips06 <128173664+SCPhillips06@users.noreply.github.com>
Date: Tue, 2 Apr 2024 19:29:25 -0400
Subject: [PATCH 15/23] sick fire animation and shooter tables

---
 Shuffleboard/shuffleboard.json                    | 15 ++++++++++++++-
 src/main/java/frc/robot/Robot.java                | 12 ++++++------
 src/main/java/frc/robot/Subsystems/Jukebox.java   |  7 ++++---
 .../java/frc/robot/utilities/LEDController.java   |  9 +++++++--
 4 files changed, 31 insertions(+), 12 deletions(-)

diff --git a/Shuffleboard/shuffleboard.json b/Shuffleboard/shuffleboard.json
index 8f77627..4dd33cb 100644
--- a/Shuffleboard/shuffleboard.json
+++ b/Shuffleboard/shuffleboard.json
@@ -291,7 +291,7 @@
               "_showGlyph": false
             }
           },
-          "9,0": {
+          "7,1": {
             "size": [
               1,
               1
@@ -342,6 +342,19 @@
               "_glyph": 148,
               "_showGlyph": false
             }
+          },
+          "5,2": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/distance to speaker",
+              "_title": "SmartDashboard/distance to speaker",
+              "_glyph": 148,
+              "_showGlyph": false
+            }
           }
         }
       }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 0688145..584a2bc 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -186,12 +186,12 @@ public void teleopJukebox() {
     }
   }
   if (_driverController.getXButtonPressed()) {
-    // _jukebox.setState(JukeboxEnum.JUKEBOX_TEST);
-    if (_jukebox.getDisableAutoSpinup()) {
-      _jukebox.enableAutoSpinup();
-    } else {
-      _jukebox.disableAutoSpinup();
-    }
+    _jukebox.setState(JukeboxEnum.JUKEBOX_TEST);
+    // if (_jukebox.getDisableAutoSpinup()) {
+    //   _jukebox.enableAutoSpinup();
+    // } else {
+    //   _jukebox.disableAutoSpinup();
+    // }
   }
 
   if (_score) {
diff --git a/src/main/java/frc/robot/Subsystems/Jukebox.java b/src/main/java/frc/robot/Subsystems/Jukebox.java
index 405b70a..6d87aee 100644
--- a/src/main/java/frc/robot/Subsystems/Jukebox.java
+++ b/src/main/java/frc/robot/Subsystems/Jukebox.java
@@ -140,9 +140,10 @@ public class Jukebox extends Subsystem{
     //private final double[] kShooterSpeeds = {35, 36, 38, 41, 44};
 
     // Old
-    private final double[] kDistanceIDs = {1.77, 2, 2.5, 3, 3.5, 4};
+    private final double[] kDistanceIDs = {1.5, 2, 2.5, 3, 3.5, 4};
     // private final double[] kShooterAngles = {5.25, 5.75, 5.85, 6.2, 6.375};
-    private final double[] kShooterAngles = {4.5, 5.1, 5.55, 5.85, 6.2, 6.35};
+    // private final double[] kShooterAngles = {4.5, 5.1, 5.55, 5.85, 6.2, 6.35};
+    private final double[] kShooterAngles = {3, 4, 5, 5.39, 5.73, 6.2};
     private final double[] kShooterSpeeds = {67, 67, 67, 67, 67, 67};
 
     private double _manualElevatorSpeed = 0;
@@ -250,7 +251,7 @@ private void configShooter() {
             kShooterFeedForwardKv
         );
 
-        _shootReadyDebouncer = new Debouncer(.04, DebounceType.kRising);
+        _shootReadyDebouncer = new Debouncer(.04, DebounceType.kBoth);
         
         _manualShooterSpeed = 0.0;
         _shooterSetpoint = 0.0;
diff --git a/src/main/java/frc/robot/utilities/LEDController.java b/src/main/java/frc/robot/utilities/LEDController.java
index 5b7f1d2..b6bdc4f 100644
--- a/src/main/java/frc/robot/utilities/LEDController.java
+++ b/src/main/java/frc/robot/utilities/LEDController.java
@@ -58,7 +58,7 @@ public class LEDController {
         // animation for SCORE --color is Green
         SCORE_LIGHTS = new ColorFlowAnimation(0, 255, 0, 0, 0.5, jukeboxNumLeds, Direction.Forward);
         // animation for PREP_SPEAKER --color is Turquoise
-        PREP_SPEAKER_LIGHTS = new FireAnimation(.5, .5, _jukebox.getShooterLeds(jukeboxNumLeds), .25, .1);
+        PREP_SPEAKER_LIGHTS = new FireAnimation(.5, .5, jukeboxNumLeds, .75, .1);
         // animation for PREP_AMP --color is Indigo
         PREP_AMP_LIGHTS = new RainbowAnimation(.5, .5, jukeboxNumLeds);
         // animation for PREP_TRAP --color is Sunset Orange
@@ -133,7 +133,12 @@ public void handleLights()
             case PREP_SPEAKER:
                 // upperCandle.clearAnimation(0);
                 // upperCandle.animate(PREP_SPEAKER_LIGHTS);
-                upperCandle.setLEDs(255, 0, 0, 0, 0, _jukebox.getShooterLeds(jukeboxNumLeds));
+                if (_jukebox.getIsReadyToScore()) {
+                    upperCandle.animate(PREP_SPEAKER_LIGHTS);
+                } else {
+                    upperCandle.clearAnimation(0);
+                    upperCandle.setLEDs(255, 0, 0, 0, 0, _jukebox.getShooterLeds(jukeboxNumLeds));
+                }
                 break;
             case PREP_AMP:
                 // upperCandle.clearAnimation(0);

From 4eda2d2e795d5bb407bc31ffd0af239ae9b3c4c9 Mon Sep 17 00:00:00 2001
From: Isaac <54126919+Aizakkuno@users.noreply.github.com>
Date: Tue, 2 Apr 2024 19:31:22 -0400
Subject: [PATCH 16/23] feed and amp aiming

---
 .../deploy/pathplanner/paths/New Path.path    | 52 +++++++++++++++++++
 .../java/frc/robot/Constants/Constants.java   | 13 +++--
 .../java/frc/robot/Enums/JukeboxEnum.java     |  3 ++
 src/main/java/frc/robot/Robot.java            |  6 +++
 .../java/frc/robot/Subsystems/Drivetrain.java |  7 +++
 .../java/frc/robot/Subsystems/Jukebox.java    | 51 +++++++++++++-----
 .../frc/robot/utilities/AllianceSpecific.java | 16 ++++++
 7 files changed, 132 insertions(+), 16 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/paths/New Path.path

diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path
new file mode 100644
index 0000000..96a01a3
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/New Path.path	
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.3,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.3000000000000003,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 6.0
+      },
+      "prevControl": {
+        "x": 3.0,
+        "y": 6.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.5,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Constants/Constants.java b/src/main/java/frc/robot/Constants/Constants.java
index 398fbe6..a0b5f32 100644
--- a/src/main/java/frc/robot/Constants/Constants.java
+++ b/src/main/java/frc/robot/Constants/Constants.java
@@ -4,6 +4,7 @@
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import frc.robot.Utilities.AllianceSpecific;
 
 public final class Constants {
     public static final double kFieldLength = 16.54;
@@ -14,11 +15,17 @@ public final class Constants {
         5.5
     );
 
-    public static final Translation2d kRedSpeaker = new Translation2d(
-        kBlueSpeaker.getX() + kFieldLength,
-        kBlueSpeaker.getY()
+    public static final Translation2d kRedSpeaker = AllianceSpecific
+        .mirrorTranslation(kBlueSpeaker);
+
+    public static final Translation2d kBlueZone = new Translation2d(
+        1.3,
+        7.0
     );
 
+    public static final Translation2d kRedZone = AllianceSpecific
+        .mirrorTranslation(kBlueZone);
+
     // public static final double k
 
     // Controller USB ports
diff --git a/src/main/java/frc/robot/Enums/JukeboxEnum.java b/src/main/java/frc/robot/Enums/JukeboxEnum.java
index c750f2d..a33c963 100644
--- a/src/main/java/frc/robot/Enums/JukeboxEnum.java
+++ b/src/main/java/frc/robot/Enums/JukeboxEnum.java
@@ -11,6 +11,9 @@ public enum JukeboxEnum
         /**Spins up the shooter for a speaker shot. */
         PREP_SPEAKER,
 
+        /* Spins up shooter and pivots for feed shot (sucessor to prep_launch) */
+        PREP_FEED,
+
         /**Spins up the shooter for a speaker shot from the podium (Doesn't use auto aim) */
         PREP_SPEAKER_PODIUM,
 
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 0688145..ba418f5 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -149,6 +149,12 @@ private void teleopDrive() {
       AllianceSpecific.getSpeaker()
     );
 
+    // amp
+    // rotation = _drivetrain.getRotationDifference(90) / 105;
+
+    // feed
+    // rotation = _drivetrain.getAngleToTarget(AllianceSpecific.getZone()) / 105
+
     SmartDashboard.putNumber("angle to speaker", speakerAngle);
     SmartDashboard.putNumber("distance to speaker", speakerDistance);
 
diff --git a/src/main/java/frc/robot/Subsystems/Drivetrain.java b/src/main/java/frc/robot/Subsystems/Drivetrain.java
index 244779e..3ac1077 100644
--- a/src/main/java/frc/robot/Subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/Subsystems/Drivetrain.java
@@ -435,4 +435,11 @@ public double getDistanceToTarget(Translation2d target) {
             .getTranslation()
             .getDistance(target);
     }
+
+    public double getRotationDifference(double angle) {
+        return Rotation2d
+            .fromDegrees(angle)
+            .minus(getGyroRotationWithOffset())
+            .getDegrees();
+    }
 }
diff --git a/src/main/java/frc/robot/Subsystems/Jukebox.java b/src/main/java/frc/robot/Subsystems/Jukebox.java
index 405b70a..902a738 100644
--- a/src/main/java/frc/robot/Subsystems/Jukebox.java
+++ b/src/main/java/frc/robot/Subsystems/Jukebox.java
@@ -145,6 +145,11 @@ public class Jukebox extends Subsystem{
     private final double[] kShooterAngles = {4.5, 5.1, 5.55, 5.85, 6.2, 6.35};
     private final double[] kShooterSpeeds = {67, 67, 67, 67, 67, 67};
 
+    private final double[] kFeedDistanceIDs = {1.77, 2, 2.5, 3, 3.5, 4};
+    // private final double[] kShooterAngles = {5.25, 5.75, 5.85, 6.2, 6.375};
+    private final double[] kFeedShooterAngles = {4.5, 5.1, 5.55, 5.85, 6.2, 6.35};
+    private final double kFeedShooterSpeed = 67;
+
     private double _manualElevatorSpeed = 0;
     private double _manualFeederSpeed = 0;
     private double _manualShooterAngleSpeed = 0;
@@ -576,19 +581,9 @@ private void prepSpeaker() {
 
         // _targetDistance = _visionSystem.getDistance();
 
-        Optional<Alliance> alliance = DriverStation.getAlliance();
-
-        if (alliance.isPresent()) {
-            kSpeaker = alliance.get() == Alliance.Blue ?
-                Constants.kBlueSpeaker :
-                Constants.kRedSpeaker;
-        }
-
-        _targetDistance = Drivetrain
-            .getInstance()
-            .getPose()
-            .getTranslation()
-            .getDistance(kSpeaker);
+        _targetDistance = Drivetrain.getInstance().getDistanceToTarget(
+            AllianceSpecific.getSpeaker()
+        );
 
         feeder();
 
@@ -613,6 +608,33 @@ private void prepSpeaker() {
         }
     }
 
+    private void prepFeed() {
+        setElevatorPosition(0);
+
+        // _targetDistance = _visionSystem.getDistance();
+
+        _targetDistance = Drivetrain.getInstance().getDistanceToTarget(
+            AllianceSpecific.getZone()
+        );
+
+        feeder();
+
+        if (_targetDistance != 0.0) {
+            double desiredShooterAngle = OneDimensionalLookup.interpLinear(
+                kFeedDistanceIDs,
+                kFeedShooterAngles,
+                _targetDistance
+            );
+
+            setShooterAngle(desiredShooterAngle);
+            setShooterSpeed(kFeedShooterSpeed);
+
+        } else {
+            setShooterAngle(Constants.KMinShooterAngle);
+            setShooterSpeed(Constants.kMaxShooterSpeed);
+        }
+    }
+
     /** Preps the speaker for a shot from the podium (Doesn't use auto aim) */
     private void prepSpeakerPodium() {
         feeder();
@@ -800,6 +822,9 @@ public void handleCurrentState()
             case PREP_SPEAKER:
                 prepSpeaker();
                 break;
+            case PREP_FEED:
+                prepFeed();
+                break;
             case PREP_SPEAKER_PODIUM:
                 prepSpeakerPodium();
                 break;
diff --git a/src/main/java/frc/robot/utilities/AllianceSpecific.java b/src/main/java/frc/robot/utilities/AllianceSpecific.java
index ca65b56..2c0ef30 100644
--- a/src/main/java/frc/robot/utilities/AllianceSpecific.java
+++ b/src/main/java/frc/robot/utilities/AllianceSpecific.java
@@ -17,7 +17,23 @@ public static Translation2d getSpeaker() {
         return isRed() ? Constants.kRedSpeaker : Constants.kBlueSpeaker;
     }
 
+    public static Translation2d getZone() {
+        return isRed() ? Constants.kRedZone : Constants.kBlueZone;
+    }
+
     public static Rotation2d getAllianceAngleOffset() {
         return Rotation2d.fromDegrees(isRed() ? 180 : 0);
     }
+
+    public static Translation2d mirrorTranslation(
+        Translation2d blueTranslation
+    ) {
+        return new Translation2d(
+            blueTranslation.getX() +
+                Constants.kFieldLength -
+                (2 * blueTranslation.getX()),
+
+            blueTranslation.getY()
+        );
+    }
 }

From cb92b5f120848ec555e328c72b606ad0c0278d45 Mon Sep 17 00:00:00 2001
From: SCPhillips06 <128173664+SCPhillips06@users.noreply.github.com>
Date: Tue, 2 Apr 2024 19:35:04 -0400
Subject: [PATCH 17/23] increased debounce time

---
 src/main/java/frc/robot/Subsystems/Jukebox.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Subsystems/Jukebox.java b/src/main/java/frc/robot/Subsystems/Jukebox.java
index 6d87aee..3bbd0e0 100644
--- a/src/main/java/frc/robot/Subsystems/Jukebox.java
+++ b/src/main/java/frc/robot/Subsystems/Jukebox.java
@@ -251,7 +251,7 @@ private void configShooter() {
             kShooterFeedForwardKv
         );
 
-        _shootReadyDebouncer = new Debouncer(.04, DebounceType.kBoth);
+        _shootReadyDebouncer = new Debouncer(.25, DebounceType.kBoth);
         
         _manualShooterSpeed = 0.0;
         _shooterSetpoint = 0.0;

From dad35c1e8ddec904395d8450530f5e0a67b397e9 Mon Sep 17 00:00:00 2001
From: Isaac <54126919+Aizakkuno@users.noreply.github.com>
Date: Tue, 2 Apr 2024 19:36:24 -0400
Subject: [PATCH 18/23] feed bind to y

---
 src/main/java/frc/robot/Robot.java | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 48b6219..c754e6a 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -163,6 +163,10 @@ private void teleopDrive() {
 
         rotation = speakerAngle / 105;
         //target angle range is -27 to 27 degrees
+    } else if (_driverController.getYButton()) {
+      rotation = _drivetrain.getAngleToTarget(
+        AllianceSpecific.getZone()
+      ) / 105;
     }
 
     // if (_driverController.getBackButton() && _driverController.getStartButton())

From a7b19afbe71c744de1bd39e54dc2fbcab28b980a Mon Sep 17 00:00:00 2001
From: SCPhillips06 <128173664+SCPhillips06@users.noreply.github.com>
Date: Wed, 3 Apr 2024 10:14:53 -0400
Subject: [PATCH 19/23] tuning

---
 src/main/deploy/pathplanner/paths/1_A.path    | 10 +++---
 src/main/deploy/pathplanner/paths/2_A.path    | 20 +++++------
 src/main/deploy/pathplanner/paths/A_1.path    |  8 ++---
 src/main/deploy/pathplanner/paths/A_2.path    |  8 ++---
 src/main/deploy/pathplanner/paths/A_3.path    | 10 +++---
 .../paths/Bottom 5-4-C To Note 4.path         | 20 +++++------
 .../paths/Bottom 5-4-C To Note 5.path         |  8 ++---
 .../paths/Bottom 5-4-C To Shoot Note 4.path   | 36 +++++++++++++------
 .../paths/Bottom 5-4-C To Shoot Note 5.path   | 18 +++++-----
 .../java/frc/robot/Autonomous/Bottom_5_4.java |  3 +-
 10 files changed, 79 insertions(+), 62 deletions(-)

diff --git a/src/main/deploy/pathplanner/paths/1_A.path b/src/main/deploy/pathplanner/paths/1_A.path
index c876a19..8b084f7 100644
--- a/src/main/deploy/pathplanner/paths/1_A.path
+++ b/src/main/deploy/pathplanner/paths/1_A.path
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 8.328810723241988,
-        "y": 7.37782702384235
+        "x": 8.284942222836145,
+        "y": 7.4309552554704394
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.908320747662531,
-        "y": 7.268558564182392
+        "x": 6.864452247256688,
+        "y": 7.321686795810481
       },
       "isLocked": false,
       "linkedName": "TOP_A_1_2_3-1"
@@ -39,7 +39,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 35.44787634349656,
+    "rotation": 20.55604521958356,
     "rotateFast": false
   },
   "reversed": false,
diff --git a/src/main/deploy/pathplanner/paths/2_A.path b/src/main/deploy/pathplanner/paths/2_A.path
index 72ba0c2..55fd2ba 100644
--- a/src/main/deploy/pathplanner/paths/2_A.path
+++ b/src/main/deploy/pathplanner/paths/2_A.path
@@ -3,29 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 8.25,
-        "y": 5.8
+        "x": 8.29962790515921,
+        "y": 5.580559282764361
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.724426116469921,
-        "y": 6.534646792546081
+        "x": 6.774054021629132,
+        "y": 6.315206075310442
       },
       "isLocked": false,
       "linkedName": "TOP_A_1_2_3-2"
     },
     {
       "anchor": {
-        "x": 2.93935385724081,
-        "y": 6.990384785778517
+        "x": 2.9246681749177457,
+        "y": 6.67
       },
       "prevControl": {
-        "x": 5.330836567521922,
-        "y": 6.862550215827963
+        "x": 5.316150885198857,
+        "y": 6.5421654300494465
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": "TOP_A_1_2_3-A"
+      "linkedName": "TOP_A_1_2_3-A late"
     }
   ],
   "rotationTargets": [],
@@ -39,7 +39,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 30.210305589469193,
+    "rotation": 26.142109373851337,
     "rotateFast": false
   },
   "reversed": false,
diff --git a/src/main/deploy/pathplanner/paths/A_1.path b/src/main/deploy/pathplanner/paths/A_1.path
index 637fca8..f854fbd 100644
--- a/src/main/deploy/pathplanner/paths/A_1.path
+++ b/src/main/deploy/pathplanner/paths/A_1.path
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 8.328810723241988,
-        "y": 7.37782702384235
+        "x": 8.284942222836145,
+        "y": 7.4309552554704394
       },
       "prevControl": {
-        "x": 5.130961637508229,
-        "y": 7.329136060505501
+        "x": 5.087093137102387,
+        "y": 7.38226429213359
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/A_2.path b/src/main/deploy/pathplanner/paths/A_2.path
index 7c54ac3..8cc17d9 100644
--- a/src/main/deploy/pathplanner/paths/A_2.path
+++ b/src/main/deploy/pathplanner/paths/A_2.path
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 8.25,
-        "y": 5.8
+        "x": 8.29962790515921,
+        "y": 5.580559282764361
       },
       "prevControl": {
-        "x": 5.546180666975807,
-        "y": 7.196524381186367
+        "x": 5.5958085721350175,
+        "y": 6.9770836639507285
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/A_3.path b/src/main/deploy/pathplanner/paths/A_3.path
index 17a4cec..3834839 100644
--- a/src/main/deploy/pathplanner/paths/A_3.path
+++ b/src/main/deploy/pathplanner/paths/A_3.path
@@ -3,16 +3,16 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.93935385724081,
-        "y": 6.990384785778517
+        "x": 2.9246681749177457,
+        "y": 6.67
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.810565385428992,
-        "y": 7.138218203628034
+        "x": 3.7958797031059275,
+        "y": 6.817833417849517
       },
       "isLocked": false,
-      "linkedName": "TOP_A_1_2_3-A"
+      "linkedName": "TOP_A_1_2_3-A late"
     },
     {
       "anchor": {
diff --git a/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Note 4.path b/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Note 4.path
index 646ec14..8ccb1ff 100644
--- a/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Note 4.path	
+++ b/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Note 4.path	
@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.6373441619921745,
-        "y": 3.630359329192267
+        "x": 1.9113560946263224,
+        "y": 4.640675614088259
       },
       "prevControl": null,
       "nextControl": {
-        "x": 4.252769217529226,
-        "y": 1.3393928867942657
+        "x": 3.526781150163374,
+        "y": 2.349709171690258
       },
       "isLocked": false,
       "linkedName": "Bottom 4-5 Start"
     },
     {
       "anchor": {
-        "x": 8.8909308774395,
-        "y": 2.775468261350217
+        "x": 8.62271291626662,
+        "y": 3.451135345920066
       },
       "prevControl": {
-        "x": 6.614650117364563,
-        "y": 1.5418709462128324
+        "x": 6.346432156191683,
+        "y": 2.2175380307826815
       },
       "nextControl": null,
       "isLocked": false,
@@ -31,7 +31,7 @@
   "rotationTargets": [
     {
       "waypointRelativePos": 0.2,
-      "rotationDegrees": 44.49504277601005,
+      "rotationDegrees": 22.82194443364705,
       "rotateFast": true
     }
   ],
@@ -45,7 +45,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 43.78112476486879,
+    "rotation": 20.13630342824809,
     "rotateFast": false
   },
   "reversed": false,
diff --git a/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Note 5.path b/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Note 5.path
index 24c7363..7e5e4de 100644
--- a/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Note 5.path	
+++ b/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Note 5.path	
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 8.469340761791367,
-        "y": 0.9719994332998654
+        "x": 8.417113363743722,
+        "y": 0.6755413868609491
       },
       "prevControl": {
-        "x": 6.519632862375568,
-        "y": 1.0306372648612432
+        "x": 6.610774438006836,
+        "y": 0.631484339891757
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Shoot Note 4.path b/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Shoot Note 4.path
index f38aab4..765a53f 100644
--- a/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Shoot Note 4.path	
+++ b/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Shoot Note 4.path	
@@ -3,25 +3,41 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 8.8909308774395,
-        "y": 2.775468261350217
+        "x": 8.62271291626662,
+        "y": 3.451135345920066
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.64402148201069,
-        "y": 1.703413451766537
+        "x": 7.345058554160041,
+        "y": 3.0252505585512077
       },
       "isLocked": false,
       "linkedName": "Bottom 4-5 Note 4 Pickup"
     },
     {
       "anchor": {
-        "x": 2.918404239090931,
-        "y": 3.5366726368260153
+        "x": 4.231693901670451,
+        "y": 3.039936240874271
       },
       "prevControl": {
-        "x": 4.739428847150874,
-        "y": 1.8918762166428356
+        "x": 6.3376567059933215,
+        "y": 2.2180970977238825
+      },
+      "nextControl": {
+        "x": 3.0274679511791946,
+        "y": 3.5098780752123226
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.131641329472284,
+        "y": 6.094558164071605
+      },
+      "prevControl": {
+        "x": 2.5575261168411423,
+        "y": 4.816903801965028
       },
       "nextControl": null,
       "isLocked": false,
@@ -30,7 +46,7 @@
   ],
   "rotationTargets": [
     {
-      "waypointRelativePos": 0.2,
+      "waypointRelativePos": 0.4,
       "rotationDegrees": -27.663471922084646,
       "rotateFast": true
     }
@@ -51,7 +67,7 @@
   "reversed": false,
   "folder": "Bottom 5-4-C",
   "previewStartingState": {
-    "rotation": 49.398705354995606,
+    "rotation": 20.13630342824809,
     "velocity": 0
   },
   "useDefaultConstraints": true
diff --git a/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Shoot Note 5.path b/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Shoot Note 5.path
index ee36824..0f733e9 100644
--- a/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Shoot Note 5.path	
+++ b/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Shoot Note 5.path	
@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 8.469340761791367,
-        "y": 0.9719994332998654
+        "x": 8.417113363743722,
+        "y": 0.6755413868609491
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.1238274993362705,
-        "y": 1.118594012203308
+        "x": 6.071600101288626,
+        "y": 0.8221359657643917
       },
       "isLocked": false,
       "linkedName": "Bottom 4-5 Note 5 Pickup"
     },
     {
       "anchor": {
-        "x": 2.6373441619921745,
-        "y": 3.630359329192267
+        "x": 1.9113560946263224,
+        "y": 4.640675614088259
       },
       "prevControl": {
-        "x": 3.768141700868111,
-        "y": 1.9268201797168305
+        "x": 3.0421536335022594,
+        "y": 2.9371364646128226
       },
       "nextControl": null,
       "isLocked": false,
@@ -45,7 +45,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -42.29388757909525,
+    "rotation": -51.952957468173985,
     "rotateFast": false
   },
   "reversed": false,
diff --git a/src/main/java/frc/robot/Autonomous/Bottom_5_4.java b/src/main/java/frc/robot/Autonomous/Bottom_5_4.java
index fa003e6..7c55561 100644
--- a/src/main/java/frc/robot/Autonomous/Bottom_5_4.java
+++ b/src/main/java/frc/robot/Autonomous/Bottom_5_4.java
@@ -129,9 +129,9 @@ public void execute()
                     _drivetrain.drive(new ChassisSpeeds());
                     nextStep();
                 }
+                break;
             // drives to 3
             default:
-                _jukebox.setState(JukeboxEnum.IDLE);
                 _drivetrain.drive(new ChassisSpeeds());
                 break;
         }
@@ -153,6 +153,7 @@ public void initialize(){
         _intake.setWantedState(IntakeState.INTAKE);
         _jukebox.setState(JukeboxEnum.IDLE);
         _loopCounter = 0;
+        _jukebox.setIdleSpeedMax();
     }
     private void nextStep() {
         _count++;

From e6e8ae29438e7937bbe2d9a8c608e6a5d5b3ce06 Mon Sep 17 00:00:00 2001
From: Isaac <54126919+Aizakkuno@users.noreply.github.com>
Date: Wed, 3 Apr 2024 12:14:14 -0400
Subject: [PATCH 20/23] mid c-b-3 tuning

---
 .../pathplanner/autos/Bottom-C-3-4-5.auto     |  8 +-
 .../deploy/pathplanner/autos/Mid C-B-3-4.auto | 14 +++-
 .../pathplanner/autos/Mid C-B-A-1 SUB.auto    |  2 +-
 src/main/deploy/pathplanner/paths/3_MID.path  | 18 ++--
 src/main/deploy/pathplanner/paths/B_3.path    |  8 +-
 src/main/deploy/pathplanner/paths/B_MID.path  | 52 ++++++++++++
 src/main/deploy/pathplanner/paths/C_MID.path  | 52 ++++++++++++
 src/main/deploy/pathplanner/paths/MID_3.path  | 52 ++++++++++++
 src/main/deploy/pathplanner/paths/MID_B.path  | 52 ++++++++++++
 src/main/deploy/pathplanner/paths/MID_C.path  | 14 ++--
 .../frc/robot/Autonomous/Mid_C_B_3_4.java     | 83 +++++++++++++------
 11 files changed, 299 insertions(+), 56 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/paths/B_MID.path
 create mode 100644 src/main/deploy/pathplanner/paths/C_MID.path
 create mode 100644 src/main/deploy/pathplanner/paths/MID_3.path
 create mode 100644 src/main/deploy/pathplanner/paths/MID_B.path

diff --git a/src/main/deploy/pathplanner/autos/Bottom-C-3-4-5.auto b/src/main/deploy/pathplanner/autos/Bottom-C-3-4-5.auto
index 5f4ea17..bfb04b4 100644
--- a/src/main/deploy/pathplanner/autos/Bottom-C-3-4-5.auto
+++ b/src/main/deploy/pathplanner/autos/Bottom-C-3-4-5.auto
@@ -14,25 +14,25 @@
         {
           "type": "path",
           "data": {
-            "pathName": "Bottom-C"
+            "pathName": null
           }
         },
         {
           "type": "path",
           "data": {
-            "pathName": "C-Line"
+            "pathName": null
           }
         },
         {
           "type": "path",
           "data": {
-            "pathName": "Line-3"
+            "pathName": null
           }
         },
         {
           "type": "path",
           "data": {
-            "pathName": "3-Line"
+            "pathName": null
           }
         }
       ]
diff --git a/src/main/deploy/pathplanner/autos/Mid C-B-3-4.auto b/src/main/deploy/pathplanner/autos/Mid C-B-3-4.auto
index 70cc0e8..5e2fd00 100644
--- a/src/main/deploy/pathplanner/autos/Mid C-B-3-4.auto	
+++ b/src/main/deploy/pathplanner/autos/Mid C-B-3-4.auto	
@@ -20,25 +20,31 @@
         {
           "type": "path",
           "data": {
-            "pathName": "C_B"
+            "pathName": "C_MID"
           }
         },
         {
           "type": "path",
           "data": {
-            "pathName": "B_3"
+            "pathName": "MID_B"
           }
         },
         {
           "type": "path",
           "data": {
-            "pathName": "3_MID"
+            "pathName": "B_MID"
           }
         },
         {
           "type": "path",
           "data": {
-            "pathName": "MID_4"
+            "pathName": "MID_3"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "3_MID"
           }
         }
       ]
diff --git a/src/main/deploy/pathplanner/autos/Mid C-B-A-1 SUB.auto b/src/main/deploy/pathplanner/autos/Mid C-B-A-1 SUB.auto
index b686a52..b99601c 100644
--- a/src/main/deploy/pathplanner/autos/Mid C-B-A-1 SUB.auto	
+++ b/src/main/deploy/pathplanner/autos/Mid C-B-A-1 SUB.auto	
@@ -56,7 +56,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "Mid C-B-A-1 SUB - Score 1"
+            "pathName": null
           }
         }
       ]
diff --git a/src/main/deploy/pathplanner/paths/3_MID.path b/src/main/deploy/pathplanner/paths/3_MID.path
index 795c7c8..9a7b2f6 100644
--- a/src/main/deploy/pathplanner/paths/3_MID.path
+++ b/src/main/deploy/pathplanner/paths/3_MID.path
@@ -3,29 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 8.666769963235812,
-        "y": 4.46444742621149
+        "x": 8.26051834784571,
+        "y": 4.323757544312242
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.765567110948872,
-        "y": 3.8697121749832677
+        "x": 6.556334631055977,
+        "y": 3.7005132136005674
       },
       "isLocked": false,
       "linkedName": "Mid C-B-3 - Note 3"
     },
     {
       "anchor": {
-        "x": 2.263812470379861,
-        "y": 5.771473152964195
+        "x": 1.8917403433857882,
+        "y": 5.950035719763017
       },
       "prevControl": {
-        "x": 3.8058091143015926,
-        "y": 5.624616329733555
+        "x": 3.3816838214933846,
+        "y": 5.326791389051343
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": "Mid C-B-3 - C Turn"
+      "linkedName": "Mid C-B-3 - Mid"
     }
   ],
   "rotationTargets": [],
diff --git a/src/main/deploy/pathplanner/paths/B_3.path b/src/main/deploy/pathplanner/paths/B_3.path
index 83a8c6a..d7514f7 100644
--- a/src/main/deploy/pathplanner/paths/B_3.path
+++ b/src/main/deploy/pathplanner/paths/B_3.path
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 8.666769963235812,
-        "y": 4.46444742621149
+        "x": 8.26051834784571,
+        "y": 4.323757544312242
       },
       "prevControl": {
-        "x": 6.317060791545553,
-        "y": 4.46444742621149
+        "x": 5.910809176155452,
+        "y": 4.323757544312242
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/B_MID.path b/src/main/deploy/pathplanner/paths/B_MID.path
new file mode 100644
index 0000000..e745690
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/B_MID.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.9980965865330664,
+        "y": 5.888958611548708
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.989122270059487,
+        "y": 5.6968427104114
+      },
+      "isLocked": false,
+      "linkedName": "Mid C-B-3 - Note B"
+    },
+    {
+      "anchor": {
+        "x": 1.8917403433857882,
+        "y": 5.950035719763017
+      },
+      "prevControl": {
+        "x": 2.425546604054034,
+        "y": 6.051790251731009
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Mid C-B-3 - Mid"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.5,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "Mid C-B-3-4",
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/C_MID.path b/src/main/deploy/pathplanner/paths/C_MID.path
new file mode 100644
index 0000000..560eac0
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/C_MID.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.6162688461334,
+        "y": 4.376333332273105
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.125456967402666,
+        "y": 4.9275254896891765
+      },
+      "isLocked": false,
+      "linkedName": "Mid C-B-3 - Note C"
+    },
+    {
+      "anchor": {
+        "x": 1.8917403433857882,
+        "y": 5.950035719763017
+      },
+      "prevControl": {
+        "x": 2.552143108729842,
+        "y": 5.535666040360404
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Mid C-B-3 - Mid"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.5,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "Mid C-B-3-4",
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/MID_3.path b/src/main/deploy/pathplanner/paths/MID_3.path
new file mode 100644
index 0000000..6878739
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/MID_3.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.8917403433857882,
+        "y": 5.950035719763017
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.255087316817576,
+        "y": 5.365744159720822
+      },
+      "isLocked": false,
+      "linkedName": "Mid C-B-3 - Mid"
+    },
+    {
+      "anchor": {
+        "x": 8.26051834784571,
+        "y": 4.323757544312242
+      },
+      "prevControl": {
+        "x": 7.199055347102391,
+        "y": 3.5252257455879086
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Mid C-B-3 - Note 3"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.5,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "Mid C-B-3-4",
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/MID_B.path b/src/main/deploy/pathplanner/paths/MID_B.path
new file mode 100644
index 0000000..e42de3a
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/MID_B.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.8917403433857882,
+        "y": 5.950035719763017
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.6415811787732713,
+        "y": 5.988988490432497
+      },
+      "isLocked": false,
+      "linkedName": "Mid C-B-3 - Mid"
+    },
+    {
+      "anchor": {
+        "x": 2.9980965865330664,
+        "y": 5.888958611548708
+      },
+      "prevControl": {
+        "x": 2.4565555180932432,
+        "y": 5.75527186641562
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Mid C-B-3 - Note B"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.5,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "Mid C-B-3-4",
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/MID_C.path b/src/main/deploy/pathplanner/paths/MID_C.path
index b92c2da..08585f8 100644
--- a/src/main/deploy/pathplanner/paths/MID_C.path
+++ b/src/main/deploy/pathplanner/paths/MID_C.path
@@ -3,16 +3,16 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.309243119380694,
-        "y": 5.565873600441298
+        "x": 1.2198050493372643,
+        "y": 5.52155524239874
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.6763851774572966,
-        "y": 5.095931766103247
+        "x": 1.586947107413868,
+        "y": 5.051613408060689
       },
       "isLocked": false,
-      "linkedName": "Mid C-B-3 - Mid"
+      "linkedName": null
     },
     {
       "anchor": {
@@ -20,8 +20,8 @@
         "y": 4.376333332273105
       },
       "prevControl": {
-        "x": 2.043527235533899,
-        "y": 4.8169038019650285
+        "x": 1.9112167287205282,
+        "y": 4.245852002973283
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/java/frc/robot/Autonomous/Mid_C_B_3_4.java b/src/main/java/frc/robot/Autonomous/Mid_C_B_3_4.java
index 913ee8a..a661a94 100644
--- a/src/main/java/frc/robot/Autonomous/Mid_C_B_3_4.java
+++ b/src/main/java/frc/robot/Autonomous/Mid_C_B_3_4.java
@@ -56,21 +56,33 @@ public void execute(){
                 if (_pathFollower.isPathFinished()){
                     // At Note C turn position (1)
                     _drivetrain.drive(new ChassisSpeeds());
+
+                    _pathFollower.startNextPath(new ChassisSpeeds(), _drivetrain.getPose());
+
                     nextStep();
                 }
 
                 break;
             case 4:
-                // Once the note is detected, we can set prep speaker.
+                _drivetrain.drive(_pathFollower.getPathTarget(_drivetrain.getPose()));
+
                 if (_jukebox.hasNote()) {
                     _jukebox.setState(JukeboxEnum.PREP_SPEAKER);                
+                }
+
+                if (_pathFollower.isPathFinished()){
+                    // At Note C turn position (1)
+                    _drivetrain.drive(new ChassisSpeeds());
+
                     nextStep();
                 }
+
                 break;
             case 5:
                 // Once ready for the shot, set score.
                 if (_jukebox.isReadyToScore()) {
                     _jukebox.setState(JukeboxEnum.SCORE);
+
                     nextStep();
                 }
                 break;
@@ -86,83 +98,100 @@ public void execute(){
 
                 break;
             case 7:
-                // Follow Path to Note B (2)
+                // Follow Path to Note C turn position (1)
                 _drivetrain.drive(_pathFollower.getPathTarget(_drivetrain.getPose()));
 
                 if (_pathFollower.isPathFinished()){
-                    // At Note B (2)
+                    // At Note C turn position (1)
                     _drivetrain.drive(new ChassisSpeeds());
+
+                    _pathFollower.startNextPath(new ChassisSpeeds(), _drivetrain.getPose());
+
                     nextStep();
                 }
 
                 break;
             case 8:
-                // Once the note is detected, we can set prep speaker.
+                _drivetrain.drive(_pathFollower.getPathTarget(_drivetrain.getPose()));
+
                 if (_jukebox.hasNote()) {
-                    _jukebox.setState(JukeboxEnum.PREP_SPEAKER);
+                    _jukebox.setState(JukeboxEnum.PREP_SPEAKER);                
+                }
+
+                if (_pathFollower.isPathFinished()){
+                    // At Note C turn position (1)
+                    _drivetrain.drive(new ChassisSpeeds());
+
                     nextStep();
                 }
 
                 break;
-            case 9:            
-                    // Once ready for the shot, set score.
-                    if (_jukebox.isReadyToScore()) {
-                        _jukebox.setState(JukeboxEnum.SCORE);
-                        nextStep();
-                    }
+            case 9:
+                // Once ready for the shot, set score.
+                if (_jukebox.isReadyToScore()) {
+                    _jukebox.setState(JukeboxEnum.SCORE);
+
+                    nextStep();
+                }
                 break;
             case 10:
                 // After the note has left the robot, transition to idle and start the next path.
                 if (!_jukebox.hasNote()) {
                     _jukebox.setState(JukeboxEnum.IDLE);
 
-                    // Start Path to Note 3 (3)
+                    // Start Path to Note B (2)
                     _pathFollower.startNextPath(new ChassisSpeeds(), _drivetrain.getPose());
                     nextStep();
                 }
+
                 break;
-            // Drives to note 3 (3)
             case 11:
+                // Follow Path to Note C turn position (1)
                 _drivetrain.drive(_pathFollower.getPathTarget(_drivetrain.getPose()));
+
                 if (_pathFollower.isPathFinished()){
-                    _pathFollower.startNextPath(new ChassisSpeeds(), _drivetrain.getPose()); 
+                    // At Note C turn position (1)
                     _drivetrain.drive(new ChassisSpeeds());
+
+                    _pathFollower.startNextPath(new ChassisSpeeds(), _drivetrain.getPose());
+
                     nextStep();
                 }
+
                 break;
-            // Drives back to Mid
             case 12:
                 _drivetrain.drive(_pathFollower.getPathTarget(_drivetrain.getPose()));
+
                 if (_jukebox.hasNote()) {
-                    _jukebox.setState(JukeboxEnum.PREP_SPEAKER);
+                    _jukebox.setState(JukeboxEnum.PREP_SPEAKER);                
                 }
+
                 if (_pathFollower.isPathFinished()){
+                    // At Note C turn position (1)
                     _drivetrain.drive(new ChassisSpeeds());
+
                     nextStep();
                 }
+
                 break;
-            // Scores at Mid
             case 13:
+                // Once ready for the shot, set score.
                 if (_jukebox.isReadyToScore()) {
                     _jukebox.setState(JukeboxEnum.SCORE);
+
                     nextStep();
                 }
                 break;
             case 14:
+                // After the note has left the robot, transition to idle and start the next path.
                 if (!_jukebox.hasNote()) {
                     _jukebox.setState(JukeboxEnum.IDLE);
-                    _pathFollower.startNextPath(new ChassisSpeeds(), _drivetrain.getPose()); 
-                    nextStep();
-                }
-                break;
-            // Drives to Note 4
-            case 15:
-                _drivetrain.drive(_pathFollower.getPathTarget(_drivetrain.getPose()));
-                if (_pathFollower.isPathFinished()){
-                    _pathFollower.startNextPath(new ChassisSpeeds(), _drivetrain.getPose()); 
-                    _drivetrain.drive(new ChassisSpeeds());
+
+                    // Start Path to Note B (2)
+                    _pathFollower.startNextPath(new ChassisSpeeds(), _drivetrain.getPose());
                     nextStep();
                 }
+
                 break;
             // DONE
             default:

From 40067c1f8c22d580f69275f7160edce67a97de1d Mon Sep 17 00:00:00 2001
From: SCPhillips06 <128173664+SCPhillips06@users.noreply.github.com>
Date: Thu, 4 Apr 2024 11:16:43 -0400
Subject: [PATCH 21/23] path fixes

---
 .../pathplanner/autos/Bottom-C-3-4-5.auto     | 43 ---------------
 .../deploy/pathplanner/autos/Mid C-B-3-4.auto | 12 ++---
 .../pathplanner/autos/Mid C-B-A-1 SUB.auto    |  4 +-
 .../paths/Mid C-B-A-3 SUB - Note 3.path       | 12 ++---
 .../paths/Mid C-B-A-3 SUB - Score 3 Yeet.path | 20 +++----
 .../paths/Mid C-B-A-3 SUB - Score 3.path      | 12 ++---
 .../deploy/pathplanner/paths/New Path.path    | 52 -------------------
 .../deploy/pathplanner/paths/TA123_T-A.path   |  2 +-
 .../frc/robot/Autonomous/Mid_C_B_3_4.java     |  1 -
 9 files changed, 31 insertions(+), 127 deletions(-)
 delete mode 100644 src/main/deploy/pathplanner/autos/Bottom-C-3-4-5.auto
 delete mode 100644 src/main/deploy/pathplanner/paths/New Path.path

diff --git a/src/main/deploy/pathplanner/autos/Bottom-C-3-4-5.auto b/src/main/deploy/pathplanner/autos/Bottom-C-3-4-5.auto
deleted file mode 100644
index bfb04b4..0000000
--- a/src/main/deploy/pathplanner/autos/Bottom-C-3-4-5.auto
+++ /dev/null
@@ -1,43 +0,0 @@
-{
-  "version": 1.0,
-  "startingPose": {
-    "position": {
-      "x": 0.7853670538059491,
-      "y": 4.459770703191282
-    },
-    "rotation": -61.144338780283555
-  },
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": null
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": null
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": null
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": null
-          }
-        }
-      ]
-    }
-  },
-  "folder": null,
-  "choreoAuto": false
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Mid C-B-3-4.auto b/src/main/deploy/pathplanner/autos/Mid C-B-3-4.auto
index 5e2fd00..8988445 100644
--- a/src/main/deploy/pathplanner/autos/Mid C-B-3-4.auto	
+++ b/src/main/deploy/pathplanner/autos/Mid C-B-3-4.auto	
@@ -14,37 +14,37 @@
         {
           "type": "path",
           "data": {
-            "pathName": "MID_C"
+            "pathName": "Mid C-B-A-3 SUB - Note C"
           }
         },
         {
           "type": "path",
           "data": {
-            "pathName": "C_MID"
+            "pathName": "Mid C-B-A-3 SUB - Score C"
           }
         },
         {
           "type": "path",
           "data": {
-            "pathName": "MID_B"
+            "pathName": "Mid C-B-A-3 SUB - Note B"
           }
         },
         {
           "type": "path",
           "data": {
-            "pathName": "B_MID"
+            "pathName": "Mid C-B-A-3 SUB - Score B"
           }
         },
         {
           "type": "path",
           "data": {
-            "pathName": "MID_3"
+            "pathName": "Mid C-B-A-3 SUB - Note 3"
           }
         },
         {
           "type": "path",
           "data": {
-            "pathName": "3_MID"
+            "pathName": "Mid C-B-A-3 SUB - Score 3 Yeet"
           }
         }
       ]
diff --git a/src/main/deploy/pathplanner/autos/Mid C-B-A-1 SUB.auto b/src/main/deploy/pathplanner/autos/Mid C-B-A-1 SUB.auto
index b99601c..01843d7 100644
--- a/src/main/deploy/pathplanner/autos/Mid C-B-A-1 SUB.auto	
+++ b/src/main/deploy/pathplanner/autos/Mid C-B-A-1 SUB.auto	
@@ -56,12 +56,12 @@
         {
           "type": "path",
           "data": {
-            "pathName": null
+            "pathName": "Mid C-B-A-1 SUB - Score 1 Normal"
           }
         }
       ]
     }
   },
-  "folder": null,
+  "folder": "Mid Autos",
   "choreoAuto": false
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Note 3.path b/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Note 3.path
index 26ffe63..8264461 100644
--- a/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Note 3.path	
+++ b/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Note 3.path	
@@ -16,16 +16,16 @@
     },
     {
       "anchor": {
-        "x": 4.628186374775031,
-        "y": 4.2978770123018135
+        "x": 4.628207324393182,
+        "y": 4.670046978734387
       },
       "prevControl": {
-        "x": 4.291274013623357,
-        "y": 4.548401588542802
+        "x": 4.2912949632415085,
+        "y": 4.920571554975376
       },
       "nextControl": {
-        "x": 5.0849090000605095,
-        "y": 3.958262752474149
+        "x": 5.084929949678661,
+        "y": 4.330432718906723
       },
       "isLocked": false,
       "linkedName": "Mid C-B-A-1 SUB - mid point"
diff --git a/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3 Yeet.path b/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3 Yeet.path
index caec868..4b53c29 100644
--- a/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3 Yeet.path	
+++ b/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3 Yeet.path	
@@ -16,28 +16,28 @@
     },
     {
       "anchor": {
-        "x": 4.628186374775031,
-        "y": 4.2978770123018135
+        "x": 4.628207324393182,
+        "y": 4.670046978734387
       },
       "prevControl": {
-        "x": 4.991504045099071,
-        "y": 4.017957375681643
+        "x": 4.991524994717222,
+        "y": 4.390127342114217
       },
       "nextControl": {
-        "x": 4.27690278809944,
-        "y": 4.5685249382481645
+        "x": 4.276923737717591,
+        "y": 4.940694904680738
       },
       "isLocked": false,
       "linkedName": "Mid C-B-A-1 SUB - mid point"
     },
     {
       "anchor": {
-        "x": 3.7264519607498547,
-        "y": 4.883418839590889
+        "x": 2.821868398656297,
+        "y": 5.448388141856785
       },
       "prevControl": {
-        "x": 4.323704624584712,
-        "y": 4.742888801041511
+        "x": 3.4191210624911577,
+        "y": 5.3078581033074075
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3.path b/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3.path
index d6453d3..f233004 100644
--- a/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3.path	
+++ b/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3.path	
@@ -16,16 +16,16 @@
     },
     {
       "anchor": {
-        "x": 4.628186374775031,
-        "y": 4.2978770123018135
+        "x": 4.628207324393182,
+        "y": 4.670046978734387
       },
       "prevControl": {
-        "x": 4.991504045099071,
-        "y": 4.017957375681643
+        "x": 4.991524994717222,
+        "y": 4.390127342114217
       },
       "nextControl": {
-        "x": 4.27690278809944,
-        "y": 4.5685249382481645
+        "x": 4.276923737717591,
+        "y": 4.940694904680738
       },
       "isLocked": false,
       "linkedName": "Mid C-B-A-1 SUB - mid point"
diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path
deleted file mode 100644
index 96a01a3..0000000
--- a/src/main/deploy/pathplanner/paths/New Path.path	
+++ /dev/null
@@ -1,52 +0,0 @@
-{
-  "version": 1.0,
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 1.3,
-        "y": 7.0
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 2.3000000000000003,
-        "y": 7.0
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 4.0,
-        "y": 6.0
-      },
-      "prevControl": {
-        "x": 3.0,
-        "y": 6.0
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [],
-  "constraintZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 4.5,
-    "maxAcceleration": 4.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": 0,
-    "rotateFast": false
-  },
-  "reversed": false,
-  "folder": null,
-  "previewStartingState": {
-    "rotation": 0,
-    "velocity": 0
-  },
-  "useDefaultConstraints": true
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/TA123_T-A.path b/src/main/deploy/pathplanner/paths/TA123_T-A.path
index 6489d6a..7597604 100644
--- a/src/main/deploy/pathplanner/paths/TA123_T-A.path
+++ b/src/main/deploy/pathplanner/paths/TA123_T-A.path
@@ -43,7 +43,7 @@
     "rotateFast": false
   },
   "reversed": false,
-  "folder": "TA123",
+  "folder": "Top A-1-2-3",
   "previewStartingState": {
     "rotation": 59.47029410006579,
     "velocity": 0
diff --git a/src/main/java/frc/robot/Autonomous/Mid_C_B_3_4.java b/src/main/java/frc/robot/Autonomous/Mid_C_B_3_4.java
index a661a94..3474d2f 100644
--- a/src/main/java/frc/robot/Autonomous/Mid_C_B_3_4.java
+++ b/src/main/java/frc/robot/Autonomous/Mid_C_B_3_4.java
@@ -188,7 +188,6 @@ public void execute(){
                     _jukebox.setState(JukeboxEnum.IDLE);
 
                     // Start Path to Note B (2)
-                    _pathFollower.startNextPath(new ChassisSpeeds(), _drivetrain.getPose());
                     nextStep();
                 }
 

From 94cff96cdf90d16f499e74b002f8b0ad6f7528c7 Mon Sep 17 00:00:00 2001
From: SCPhillips06 <128173664+SCPhillips06@users.noreply.github.com>
Date: Thu, 4 Apr 2024 19:23:30 -0400
Subject: [PATCH 22/23] states changes

---
 Shuffleboard/shuffleboard.json                |  2 +-
 .../deploy/pathplanner/autos/Mid C-B-3-4.auto |  4 +-
 .../deploy/pathplanner/autos/Top A-1-2-3.auto |  6 ++
 src/main/deploy/pathplanner/paths/1_A.path    |  2 +-
 src/main/deploy/pathplanner/paths/2_A.path    |  4 +-
 src/main/deploy/pathplanner/paths/A_1.path    | 12 ++--
 src/main/deploy/pathplanner/paths/A_2.path    |  4 +-
 src/main/deploy/pathplanner/paths/A_TOP.path  | 58 +++++++++++++++++++
 .../paths/Mid C-B-A-3 SUB - Note 3.path       |  6 +-
 .../paths/Mid C-B-A-3 SUB - Score 3 Yeet.path | 16 ++---
 .../paths/Mid C-B-A-3 SUB - Score 3.path      |  6 +-
 src/main/deploy/pathplanner/paths/TOP_A.path  |  2 +-
 .../frc/robot/Autonomous/Mid_C_B_3_4.java     |  8 +--
 .../frc/robot/Autonomous/Mid_C_B_A_1.java     |  1 -
 .../frc/robot/Autonomous/TOP_A_1_2_3.java     | 17 ++++--
 src/main/java/frc/robot/Robot.java            | 14 +++--
 .../java/frc/robot/Subsystems/Jukebox.java    | 13 +++--
 .../frc/robot/utilities/AllianceSpecific.java |  5 +-
 .../frc/robot/utilities/LEDController.java    |  1 +
 19 files changed, 131 insertions(+), 50 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/paths/A_TOP.path

diff --git a/Shuffleboard/shuffleboard.json b/Shuffleboard/shuffleboard.json
index 4dd33cb..3917dd8 100644
--- a/Shuffleboard/shuffleboard.json
+++ b/Shuffleboard/shuffleboard.json
@@ -519,6 +519,6 @@
     "x": -8.0,
     "y": -8.0,
     "width": 1382.0,
-    "height": 784.0
+    "height": 736.0
   }
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Mid C-B-3-4.auto b/src/main/deploy/pathplanner/autos/Mid C-B-3-4.auto
index 8988445..8cee562 100644
--- a/src/main/deploy/pathplanner/autos/Mid C-B-3-4.auto	
+++ b/src/main/deploy/pathplanner/autos/Mid C-B-3-4.auto	
@@ -2,8 +2,8 @@
   "version": 1.0,
   "startingPose": {
     "position": {
-      "x": 1.3186789318946823,
-      "y": 5.567111941824834
+      "x": 1.27,
+      "y": 5.53
     },
     "rotation": 0
   },
diff --git a/src/main/deploy/pathplanner/autos/Top A-1-2-3.auto b/src/main/deploy/pathplanner/autos/Top A-1-2-3.auto
index 47db1a2..50d246a 100644
--- a/src/main/deploy/pathplanner/autos/Top A-1-2-3.auto	
+++ b/src/main/deploy/pathplanner/autos/Top A-1-2-3.auto	
@@ -17,6 +17,12 @@
             "pathName": "TOP_A"
           }
         },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "A_TOP"
+          }
+        },
         {
           "type": "path",
           "data": {
diff --git a/src/main/deploy/pathplanner/paths/1_A.path b/src/main/deploy/pathplanner/paths/1_A.path
index 8b084f7..3fc6f38 100644
--- a/src/main/deploy/pathplanner/paths/1_A.path
+++ b/src/main/deploy/pathplanner/paths/1_A.path
@@ -39,7 +39,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 20.55604521958356,
+    "rotation": 29.24882633654691,
     "rotateFast": false
   },
   "reversed": false,
diff --git a/src/main/deploy/pathplanner/paths/2_A.path b/src/main/deploy/pathplanner/paths/2_A.path
index 55fd2ba..c415f01 100644
--- a/src/main/deploy/pathplanner/paths/2_A.path
+++ b/src/main/deploy/pathplanner/paths/2_A.path
@@ -4,12 +4,12 @@
     {
       "anchor": {
         "x": 8.29962790515921,
-        "y": 5.580559282764361
+        "y": 5.6539876943796825
       },
       "prevControl": null,
       "nextControl": {
         "x": 6.774054021629132,
-        "y": 6.315206075310442
+        "y": 6.388634486925763
       },
       "isLocked": false,
       "linkedName": "TOP_A_1_2_3-2"
diff --git a/src/main/deploy/pathplanner/paths/A_1.path b/src/main/deploy/pathplanner/paths/A_1.path
index f854fbd..676ef88 100644
--- a/src/main/deploy/pathplanner/paths/A_1.path
+++ b/src/main/deploy/pathplanner/paths/A_1.path
@@ -3,16 +3,16 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.93935385724081,
-        "y": 6.990384785778517
+        "x": 0.9127296966579629,
+        "y": 6.623242727701913
       },
       "prevControl": null,
       "nextControl": {
-        "x": 4.161140321454306,
-        "y": 7.216123744966993
+        "x": 2.1345161608714593,
+        "y": 6.848981686890389
       },
       "isLocked": false,
-      "linkedName": "TOP_A_1_2_3-A"
+      "linkedName": "TOP_A_1_2_3-TOP"
     },
     {
       "anchor": {
@@ -45,7 +45,7 @@
   "reversed": false,
   "folder": "Top A-1-2-3",
   "previewStartingState": {
-    "rotation": 32.0,
+    "rotation": 59.85861444792467,
     "velocity": 0
   },
   "useDefaultConstraints": true
diff --git a/src/main/deploy/pathplanner/paths/A_2.path b/src/main/deploy/pathplanner/paths/A_2.path
index 8cc17d9..14af31c 100644
--- a/src/main/deploy/pathplanner/paths/A_2.path
+++ b/src/main/deploy/pathplanner/paths/A_2.path
@@ -17,11 +17,11 @@
     {
       "anchor": {
         "x": 8.29962790515921,
-        "y": 5.580559282764361
+        "y": 5.6539876943796825
       },
       "prevControl": {
         "x": 5.5958085721350175,
-        "y": 6.9770836639507285
+        "y": 7.05051207556605
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/A_TOP.path b/src/main/deploy/pathplanner/paths/A_TOP.path
new file mode 100644
index 0000000..03a0bea
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/A_TOP.path
@@ -0,0 +1,58 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.93935385724081,
+        "y": 6.990384785778517
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.2491267880622807,
+        "y": 6.902270691847475
+      },
+      "isLocked": false,
+      "linkedName": "TOP_A_1_2_3-A"
+    },
+    {
+      "anchor": {
+        "x": 0.9127296966579629,
+        "y": 6.623242727701913
+      },
+      "prevControl": {
+        "x": 1.4854713072629475,
+        "y": 6.667299774678448
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "TOP_A_1_2_3-TOP"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5,
+      "rotationDegrees": 57.15314553496158,
+      "rotateFast": false
+    }
+  ],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.5,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 59.85861444792467,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "Top A-1-2-3",
+  "previewStartingState": {
+    "rotation": 34.85445556626899,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Note 3.path b/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Note 3.path
index 8264461..bcd1cea 100644
--- a/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Note 3.path	
+++ b/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Note 3.path	
@@ -16,15 +16,15 @@
     },
     {
       "anchor": {
-        "x": 4.628207324393182,
+        "x": 4.540093230454797,
         "y": 4.670046978734387
       },
       "prevControl": {
-        "x": 4.2912949632415085,
+        "x": 4.203180869303123,
         "y": 4.920571554975376
       },
       "nextControl": {
-        "x": 5.084929949678661,
+        "x": 4.996815855740276,
         "y": 4.330432718906723
       },
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3 Yeet.path b/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3 Yeet.path
index 4b53c29..c8d998a 100644
--- a/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3 Yeet.path	
+++ b/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3 Yeet.path	
@@ -16,15 +16,15 @@
     },
     {
       "anchor": {
-        "x": 4.628207324393182,
+        "x": 4.540093230454797,
         "y": 4.670046978734387
       },
       "prevControl": {
-        "x": 4.991524994717222,
+        "x": 4.903410900778837,
         "y": 4.390127342114217
       },
       "nextControl": {
-        "x": 4.276923737717591,
+        "x": 4.188809643779206,
         "y": 4.940694904680738
       },
       "isLocked": false,
@@ -32,12 +32,12 @@
     },
     {
       "anchor": {
-        "x": 2.821868398656297,
-        "y": 5.448388141856785
+        "x": 1.4120428956421427,
+        "y": 5.551187918118234
       },
       "prevControl": {
-        "x": 3.4191210624911577,
-        "y": 5.3078581033074075
+        "x": 2.009295559477004,
+        "y": 5.4106578795688565
       },
       "nextControl": null,
       "isLocked": false,
@@ -55,7 +55,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -5.648247373735291,
+    "rotation": 1.5452967179226196,
     "rotateFast": false
   },
   "reversed": false,
diff --git a/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3.path b/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3.path
index f233004..4843d30 100644
--- a/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3.path	
+++ b/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3.path	
@@ -16,15 +16,15 @@
     },
     {
       "anchor": {
-        "x": 4.628207324393182,
+        "x": 4.540093230454797,
         "y": 4.670046978734387
       },
       "prevControl": {
-        "x": 4.991524994717222,
+        "x": 4.903410900778837,
         "y": 4.390127342114217
       },
       "nextControl": {
-        "x": 4.276923737717591,
+        "x": 4.188809643779206,
         "y": 4.940694904680738
       },
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/TOP_A.path b/src/main/deploy/pathplanner/paths/TOP_A.path
index 9869642..9661e2a 100644
--- a/src/main/deploy/pathplanner/paths/TOP_A.path
+++ b/src/main/deploy/pathplanner/paths/TOP_A.path
@@ -45,7 +45,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 22.742478873939014,
+    "rotation": 30.963756532073536,
     "rotateFast": false
   },
   "reversed": false,
diff --git a/src/main/java/frc/robot/Autonomous/Mid_C_B_3_4.java b/src/main/java/frc/robot/Autonomous/Mid_C_B_3_4.java
index 3474d2f..6761feb 100644
--- a/src/main/java/frc/robot/Autonomous/Mid_C_B_3_4.java
+++ b/src/main/java/frc/robot/Autonomous/Mid_C_B_3_4.java
@@ -28,7 +28,7 @@ public void execute(){
         switch (_count) {
             case 0:
                 // Initial Position - Prep for Speaker Shot
-                _jukebox.setState(JukeboxEnum.PREP_SPEAKER);
+                _jukebox.setState(JukeboxEnum.PREP_SPEAKER_SUBWOOFER);
                 nextStep();
                 break;
             case 1:
@@ -67,7 +67,7 @@ public void execute(){
                 _drivetrain.drive(_pathFollower.getPathTarget(_drivetrain.getPose()));
 
                 if (_jukebox.hasNote()) {
-                    _jukebox.setState(JukeboxEnum.PREP_SPEAKER);                
+                    _jukebox.setState(JukeboxEnum.PREP_SPEAKER_SUBWOOFER);                
                 }
 
                 if (_pathFollower.isPathFinished()){
@@ -115,7 +115,7 @@ public void execute(){
                 _drivetrain.drive(_pathFollower.getPathTarget(_drivetrain.getPose()));
 
                 if (_jukebox.hasNote()) {
-                    _jukebox.setState(JukeboxEnum.PREP_SPEAKER);                
+                    _jukebox.setState(JukeboxEnum.PREP_SPEAKER_SUBWOOFER);                
                 }
 
                 if (_pathFollower.isPathFinished()){
@@ -163,7 +163,7 @@ public void execute(){
                 _drivetrain.drive(_pathFollower.getPathTarget(_drivetrain.getPose()));
 
                 if (_jukebox.hasNote()) {
-                    _jukebox.setState(JukeboxEnum.PREP_SPEAKER);                
+                    _jukebox.setState(JukeboxEnum.PREP_SPEAKER_SUBWOOFER);                
                 }
 
                 if (_pathFollower.isPathFinished()){
diff --git a/src/main/java/frc/robot/Autonomous/Mid_C_B_A_1.java b/src/main/java/frc/robot/Autonomous/Mid_C_B_A_1.java
index f7e9a67..d557186 100644
--- a/src/main/java/frc/robot/Autonomous/Mid_C_B_A_1.java
+++ b/src/main/java/frc/robot/Autonomous/Mid_C_B_A_1.java
@@ -238,7 +238,6 @@ public void initialize(){
         // Set initial subsystem states. We should never need to change the Intake state as it is autonomous.
         _intake.setWantedState(IntakeState.INTAKE);
         _jukebox.setState(JukeboxEnum.IDLE);
-        _jukebox.setIdleSpeedMax();
     }
 
     private void nextStep() {
diff --git a/src/main/java/frc/robot/Autonomous/TOP_A_1_2_3.java b/src/main/java/frc/robot/Autonomous/TOP_A_1_2_3.java
index 54c6500..cf34d64 100644
--- a/src/main/java/frc/robot/Autonomous/TOP_A_1_2_3.java
+++ b/src/main/java/frc/robot/Autonomous/TOP_A_1_2_3.java
@@ -35,7 +35,7 @@ public void execute() {
         switch (_step) {
             case 0:
                 // Initial Position - Prep for Speaker Shot
-                _jukebox.setState(JukeboxEnum.PREP_SPEAKER);
+                _jukebox.setState(JukeboxEnum.PREP_SPEAKER_SUBWOOFER);
                 nextStep();
                 break;
             case 1:
@@ -62,6 +62,7 @@ public void execute() {
 
                 if (_pathFollower.isPathFinished()){
                     // At Note A (1)
+                    _pathFollower.startNextPath(new ChassisSpeeds(), _drivetrain.getPose());
                     _drivetrain.drive(new ChassisSpeeds());
                     nextStep();
                 }
@@ -69,10 +70,18 @@ public void execute() {
                 break;
             case 4:
                 // Once the note is detected, we can set prep speaker.
+                _drivetrain.drive(_pathFollower.getPathTarget(_drivetrain.getPose()));
+
                 if (_jukebox.hasNote()) {
-                    _jukebox.setState(JukeboxEnum.PREP_SPEAKER);                
+                    _jukebox.setState(JukeboxEnum.PREP_SPEAKER_SUBWOOFER);
+                }
+
+                if (_pathFollower.isPathFinished()){
+                    // At Note A (2)
+                    _drivetrain.drive(new ChassisSpeeds());
                     nextStep();
                 }
+
                 break;
             case 5:
                 // Once ready for the shot, set score.
@@ -116,7 +125,7 @@ public void execute() {
                 _drivetrain.drive(_pathFollower.getPathTarget(_drivetrain.getPose()));
 
                 if (_jukebox.hasNote()) {
-                    _jukebox.setState(JukeboxEnum.PREP_SPEAKER);
+                    _jukebox.setState(JukeboxEnum.PREP_SPEAKER_PODIUM);
                 }
 
                 if (_pathFollower.isPathFinished()){
@@ -167,7 +176,7 @@ public void execute() {
                 _drivetrain.drive(_pathFollower.getPathTarget(_drivetrain.getPose()));
 
                 if (_jukebox.hasNote()) {
-                    _jukebox.setState(JukeboxEnum.PREP_SPEAKER);
+                    _jukebox.setState(JukeboxEnum.PREP_SPEAKER_PODIUM);
                 }
 
                 if (_pathFollower.isPathFinished()){
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index c754e6a..8d8d0b0 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -163,11 +163,17 @@ private void teleopDrive() {
 
         rotation = speakerAngle / 105;
         //target angle range is -27 to 27 degrees
-    } else if (_driverController.getYButton()) {
-      rotation = _drivetrain.getAngleToTarget(
+    } else if (_driverController.getLeftBumper()) {
+      var feedAngle = _drivetrain.getAngleToTarget(
         AllianceSpecific.getZone()
-      ) / 105;
-    }
+      );
+      
+      SmartDashboard.putNumber("angle to zone", feedAngle);
+        rotation = feedAngle / 105;
+      } 
+    // else if (_driverController.getLeftBumper()) {
+    //   rotation = _drivetrain.getRotationDifference(90);
+    // }
 
     // if (_driverController.getBackButton() && _driverController.getStartButton())
     // {
diff --git a/src/main/java/frc/robot/Subsystems/Jukebox.java b/src/main/java/frc/robot/Subsystems/Jukebox.java
index fc10384..571f9c6 100644
--- a/src/main/java/frc/robot/Subsystems/Jukebox.java
+++ b/src/main/java/frc/robot/Subsystems/Jukebox.java
@@ -82,13 +82,13 @@ public class Jukebox extends Subsystem{
     private final double kPhotoEyeDebounceTime = 0.04;
 
     // Set Points
-    private final double kTrapElevatorPosition = 60;
-    private final double kTrapShooterAngle = 14;
+    private final double kTrapElevatorPosition = 79;
+    private final double kTrapShooterAngle = 11;
     private final double kExtendClimbElevatorPosition = 83; // change this
     private final double kExtendClimbShooterAngle = 4;
     private final double kAmpElevatorPosition = 60;
     private final double kFeedShooterAngle = 7;
-    private final double kPodiumSpeakerShotAngle = 5.9;
+    private final double kPodiumSpeakerShotAngle = 6;
     private final double kPodiumSpeakerShotSpeed = 38;
     private final double kLineSpeakerShotAngle = 5.2;
     private final double kLineSpeakerShotSpeed = 35;
@@ -148,7 +148,8 @@ public class Jukebox extends Subsystem{
 
     private final double[] kFeedDistanceIDs = {1.77, 2, 2.5, 3, 3.5, 4};
     // private final double[] kShooterAngles = {5.25, 5.75, 5.85, 6.2, 6.375};
-    private final double[] kFeedShooterAngles = {4.5, 5.1, 5.55, 5.85, 6.2, 6.35};
+    // private final double[] kFeedShooterAngles = {4.5, 5.1, 5.55, 5.85, 6.2, 6.35};
+    private final double[] kFeedShooterAngles = {4.5, 5.15, 5.60, 5.90, 6.25, 6.40}; // might work
     private final double kFeedShooterSpeed = 67;
 
     private double _manualElevatorSpeed = 0;
@@ -640,14 +641,14 @@ private void prepFeed() {
     private void prepSpeakerPodium() {
         feeder();
         setShooterAngle(kPodiumSpeakerShotAngle);
-        setShooterSpeed(kPodiumSpeakerShotSpeed);
+        setShooterSpeed(67);
         setElevatorPosition(0);
     }
 
     private void prepSpeakerLine() {
         feeder();
         setShooterAngle(kLineSpeakerShotAngle);
-        setShooterSpeed(kLineSpeakerShotSpeed);
+        setShooterSpeed(67);
         setElevatorPosition(0);
     }
 
diff --git a/src/main/java/frc/robot/utilities/AllianceSpecific.java b/src/main/java/frc/robot/utilities/AllianceSpecific.java
index 2c0ef30..b7ccbb5 100644
--- a/src/main/java/frc/robot/utilities/AllianceSpecific.java
+++ b/src/main/java/frc/robot/utilities/AllianceSpecific.java
@@ -10,7 +10,7 @@
 
 public class AllianceSpecific {
     private static boolean isRed() {
-        return DriverStation.getAlliance().get() == Alliance.Red;
+        return DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red;
     }
 
     public static Translation2d getSpeaker() {
@@ -22,7 +22,8 @@ public static Translation2d getZone() {
     }
 
     public static Rotation2d getAllianceAngleOffset() {
-        return Rotation2d.fromDegrees(isRed() ? 180 : 0);
+        //return Rotation2d.fromDegrees(isRed() ? 180 : 0);
+        return Rotation2d.fromDegrees(180);
     }
 
     public static Translation2d mirrorTranslation(
diff --git a/src/main/java/frc/robot/utilities/LEDController.java b/src/main/java/frc/robot/utilities/LEDController.java
index b6bdc4f..e37c7c6 100644
--- a/src/main/java/frc/robot/utilities/LEDController.java
+++ b/src/main/java/frc/robot/utilities/LEDController.java
@@ -123,6 +123,7 @@ public void handleLights()
                 }
                 break;
             case SCORE:
+                upperCandle.clearAnimation(0);
                 upperCandle.setLEDs(0, 255, 255, 0, 0, jukeboxNumLeds);
                 break;
             case PREP_LAUNCH:

From 6eca50149846b2ea45fdc924cdebd3a873470ca7 Mon Sep 17 00:00:00 2001
From: SCPhillips06 <128173664+SCPhillips06@users.noreply.github.com>
Date: Thu, 25 Apr 2024 19:20:10 -0400
Subject: [PATCH 23/23] States changes

---
 src/main/deploy/pathplanner/autos/Mid C-B-3-4.auto   | 6 ++++++
 src/main/java/frc/robot/Autonomous/Mid_C_B_3_4.java  | 9 +++++++++
 src/main/java/frc/robot/Robot.java                   | 2 +-
 src/main/java/frc/robot/Subsystems/Jukebox.java      | 2 +-
 src/main/java/frc/robot/utilities/LEDController.java | 9 +++++++++
 5 files changed, 26 insertions(+), 2 deletions(-)

diff --git a/src/main/deploy/pathplanner/autos/Mid C-B-3-4.auto b/src/main/deploy/pathplanner/autos/Mid C-B-3-4.auto
index 8cee562..7d3b3c8 100644
--- a/src/main/deploy/pathplanner/autos/Mid C-B-3-4.auto	
+++ b/src/main/deploy/pathplanner/autos/Mid C-B-3-4.auto	
@@ -46,6 +46,12 @@
           "data": {
             "pathName": "Mid C-B-A-3 SUB - Score 3 Yeet"
           }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Mid C-B-A-3 SUB - Note 3"
+          }
         }
       ]
     }
diff --git a/src/main/java/frc/robot/Autonomous/Mid_C_B_3_4.java b/src/main/java/frc/robot/Autonomous/Mid_C_B_3_4.java
index 6761feb..05cd60a 100644
--- a/src/main/java/frc/robot/Autonomous/Mid_C_B_3_4.java
+++ b/src/main/java/frc/robot/Autonomous/Mid_C_B_3_4.java
@@ -189,6 +189,15 @@ public void execute(){
 
                     // Start Path to Note B (2)
                     nextStep();
+                    _pathFollower.startNextPath(new ChassisSpeeds(), _drivetrain.getPose());
+                }
+                break;
+            case 15:
+                _drivetrain.drive(_pathFollower.getPathTarget(_drivetrain.getPose()));
+                if (_pathFollower.isPathFinished()){
+                    // At Note C turn position (1)
+                    _drivetrain.drive(new ChassisSpeeds());
+                    nextStep();
                 }
 
                 break;
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 8d8d0b0..9a1e28f 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -191,7 +191,7 @@ public void teleopJukebox() {
       _jukebox.setState(JukeboxEnum.PREP_AMP);
       //_jukebox.setState(JukeboxEnum.PREP_SPEAKER_LINE);
     } else if (_operatorController.getXButton()) {
-      _jukebox.setState(JukeboxEnum.PREP_SPEAKER_LINE);
+      _jukebox.setState(JukeboxEnum.PREP_SPEAKER_SUBWOOFER);
       //_jukebox.setState(JukeboxEnum.PREP_AMP);
     } else if (_operatorController.getYButton()) {
       _jukebox.setState(JukeboxEnum.PREP_SPEAKER_PODIUM);
diff --git a/src/main/java/frc/robot/Subsystems/Jukebox.java b/src/main/java/frc/robot/Subsystems/Jukebox.java
index 571f9c6..be3fe56 100644
--- a/src/main/java/frc/robot/Subsystems/Jukebox.java
+++ b/src/main/java/frc/robot/Subsystems/Jukebox.java
@@ -589,7 +589,7 @@ private void prepSpeaker() {
 
         feeder();
 
-        if (_targetDistance != 0.0) {
+        if (_targetDistance > 1.75) {
             double desiredShooterAngle = OneDimensionalLookup.interpLinear(
                 kDistanceIDs,
                 kShooterAngles,
diff --git a/src/main/java/frc/robot/utilities/LEDController.java b/src/main/java/frc/robot/utilities/LEDController.java
index e37c7c6..fa58cc9 100644
--- a/src/main/java/frc/robot/utilities/LEDController.java
+++ b/src/main/java/frc/robot/utilities/LEDController.java
@@ -127,9 +127,17 @@ public void handleLights()
                 upperCandle.setLEDs(0, 255, 255, 0, 0, jukeboxNumLeds);
                 break;
             case PREP_LAUNCH:
+                upperCandle.clearAnimation(0);
                 upperCandle.setLEDs(170, 51, 106, 0, 0, _jukebox.getShooterLeds(jukeboxNumLeds));
                 break;
+            case PREP_SPEAKER_SUBWOOFER:
+                upperCandle.clearAnimation(0);
+                upperCandle.setLEDs(0, 0, 255, 0, 0, _jukebox.getShooterLeds(jukeboxNumLeds));
+                break;
             case PREP_SPEAKER_PODIUM:
+                upperCandle.clearAnimation(0);
+                upperCandle.setLEDs(255, 0, 255, 0, 0, _jukebox.getShooterLeds(jukeboxNumLeds));
+                break;
             case PREP_SPEAKER_LINE:
             case PREP_SPEAKER:
                 // upperCandle.clearAnimation(0);
@@ -144,6 +152,7 @@ public void handleLights()
             case PREP_AMP:
                 // upperCandle.clearAnimation(0);
                 // upperCandle.animate(PREP_AMP_LIGHTS);
+                upperCandle.clearAnimation(0);
                 upperCandle.setLEDs(255, 153, 51, 0, 0, jukeboxNumLeds);
                 break;
             case PREP_TRAP: