From 0926d77d06b71a48731491c62c82e656e69602d0 Mon Sep 17 00:00:00 2001
From: C4llSqin <83995467+C4llSqin@users.noreply.github.com>
Date: Sat, 3 Feb 2024 12:31:46 -0700
Subject: [PATCH] tmp code

---
 src/main/java/frc4388/robot/Constants.java    | 19 ++++++++++++++-----
 .../java/frc4388/robot/subsystems/Vision.java |  6 ++++--
 2 files changed, 18 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index da28e1c..646c675 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -127,16 +127,25 @@ public final class Constants {
         public static final double H_FOV = 59.6;
         public static final double V_FOV = 45.7;
     
-        public static final double LIME_HEIGHT = 6.0;
-        public static final double LIME_ANGLE = 55.0;
+        // public static final double LIME_HEIGHT = 6.0;
+        // public static final double LIME_ANGLE = 55.0;
     
         // public static final double HIGH_TARGET_HEIGHT = 46.0;
-        public static final double HIGH_TAPE_HEIGHT = 44.0;
+        //public static final double HIGH_TAPE_HEIGHT = 44.0;
         
         // public static final double MID_TARGET_HEIGHT = 34.0;
-        public static final double MID_TAPE_HEIGHT = 24.0;
+        //public static final double MID_TAPE_HEIGHT = 24.0;
     
-        public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value
+        public static final double[] LIME_POS = {
+          13.0, // X Pos offset.
+          0.0, // Y Pos offset.
+          14.0, // Z Pos offset.
+          0.0, // X Rot offset.
+          0.0, // Y Rot offset.
+          0.0  // Z Rot offset.
+        };
+
+        //public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value
     
       }
     
diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java
index 9917776..af5a09a 100644
--- a/src/main/java/frc4388/robot/subsystems/Vision.java
+++ b/src/main/java/frc4388/robot/subsystems/Vision.java
@@ -5,17 +5,19 @@ import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import frc4388.robot.Constants.VisionConstants;
 
 public class Vision {
+    // camerapose_robotspace_set "3D transform of the camera in the coordinate system of the robot (array (6))"
     private final NetworkTableEntry nt_ll_tx;
     private final NetworkTableEntry nt_ll_ty;
     private final NetworkTableEntry nt_ll_ta;
     private final NetworkTableEntry nt_ll_ts;
 
     public Vision() {
-        final var tagTable = NetworkTableInstance.getDefault().getTable("limelight`");
+        final var tagTable = NetworkTableInstance.getDefault().getTable("limelight");
 
-        nt_ll_tx = tagTable.getEntry("tx"); //TODO
+        nt_ll_tx = tagTable.getEntry("camerapose_robotspace_set").setDoubleArray(VisionConstants); //TODO
         nt_ll_ty = tagTable.getEntry("ty"); //TODO
         nt_ll_ta = tagTable.getEntry("ta"); //TODO
         nt_ll_ts = tagTable.getEntry("ts"); //TODO