diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000..3fab91a
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,5 @@
+{
+    "python.analysis.extraPaths": [
+        "./FRC_Fiducial_Tracking"
+    ]
+}
\ No newline at end of file
diff --git a/FRC_Fiducial_Tracking/Pose_Estimation.py b/FRC_Fiducial_Tracking/Pose_Estimation.py
index b82c784..871a3c0 100644
--- a/FRC_Fiducial_Tracking/Pose_Estimation.py
+++ b/FRC_Fiducial_Tracking/Pose_Estimation.py
@@ -13,6 +13,7 @@ class PoseEstimation:
         self.visible_tags = len(data_array)
 
     def getCoords(self):
+
         coord_array = []
         if(len(self.visible_tags)>1):
             for i in self.visible_tags:
@@ -29,6 +30,21 @@ class PoseEstimation:
         
                 self.orientation = [self.data_array[i].tag_id, rvecPitch, rvecRoll, rvecYaw]
                 coord_array.append(self.orientation)
-        
+        elif(len(self.visible_tags) == 2):
+            tvecXCoord 
 
         return coord_array
+    
+    def cam_pose_to_robot(self, tag, robot_space_pose):
+        robot_tvec_x = cam_rotation_comp(tag.tvec_x, tag.tvec_z, robot_space_pose[4])[0]
+        robot_tvec_y = cam_rotation_comp(tag.tvec_y, tag.tvec_z, robot_space_pose[3])[0]
+        robot_tvec_z = cam_rotation_comp(tag.tvec_y, tag.tvec_z, robot_space_pose[3])[1]
+
+def distance(d1x, d1y, d2x, d2y):
+    return math.sqrt((d2x - d1x)**2 + (d2y - d1y)**2)
+
+def cam_rotation_comp(opposite, adjacent, tilt_theta):
+    opposite_out = math.sin(tilt_theta - math.atan(opposite/adjacent)) * math.hypot(opposite, adjacent)
+    adjacent_out = math.cos(tilt_theta - math.atan(opposite/adjacent)) * math.hypot(opposite, adjacent)
+    return [opposite_out, adjacent_out]
+