From 8740431940b1b5cf0e4974fea4aa7f1e5e1e384c Mon Sep 17 00:00:00 2001
From: Tylr-J42 <pi@PilerJac.home>
Date: Mon, 9 Oct 2023 22:25:00 -0400
Subject: [PATCH] math for triangulating robot coords

---
 FRC_Fiducial_Tracking/Pose_Estimation.py         |  13 ++++++++++---
 .../__pycache__/Pose_Estimation.cpython-39.pyc   | Bin 0 -> 3189 bytes
 2 files changed, 10 insertions(+), 3 deletions(-)
 create mode 100644 FRC_Fiducial_Tracking/__pycache__/Pose_Estimation.cpython-39.pyc

diff --git a/FRC_Fiducial_Tracking/Pose_Estimation.py b/FRC_Fiducial_Tracking/Pose_Estimation.py
index 13d73fe..d0d391d 100644
--- a/FRC_Fiducial_Tracking/Pose_Estimation.py
+++ b/FRC_Fiducial_Tracking/Pose_Estimation.py
@@ -36,17 +36,24 @@ class PoseEstimation:
             tag0Tvec = cam_pose_to_robot_tvec(self.data_array[0], self.robo_space_pose)
             tag1Tvec = cam_pose_to_robot_tvec(self.data_array[1], self.robo_space_pose)
 
-            tag0Coords = [self.tag_coords[self.data_array[0].tag_id][0], self.tag_coords[self.data_array[0].tag_id][2], self.tag_coords[self.data_array[0].tag_id][3]]
-            tag1Coords = [self.tag_coords[self.data_array[1].tag_id][0], self.tag_coords[self.data_array[1].tag_id][2], self.tag_coords[self.data_array[1].tag_id][3]]
+            tag0Coords = [self.tag_coords[self.data_array[0].tag_id][1], self.tag_coords[self.data_array[0].tag_id][2], self.tag_coords[self.data_array[0].tag_id][3]]
+            tag1Coords = [self.tag_coords[self.data_array[1].tag_id][1], self.tag_coords[self.data_array[1].tag_id][2], self.tag_coords[self.data_array[1].tag_id][3]]
             
             tag_line_dist = distance(tag0Coords[0], tag0Coords[1], tag1Coords[0], tag1Coords[1])
             tag0_cam_dist = math.hypot(tag0Tvec[0], tag0Tvec[2])
             tag1_cam_dist = math.hypot(tag1Tvec[0], tag1Tvec[2])
 
-            cam_angle = math.toDegrees(math.atan(tag0Tvec[0]/tag0Tvec[2])-math.atan(tag1Tvec[0]/tag1Tvec[2]))
+            tag_line_angle = slope_angle(tag0Coords[0], tag0Coords[1], tag1Coords[0], tag1Coords[1])
+
+            cam_angle = math.abs(math.toDegrees(math.atan(tag0Tvec[0]/tag0Tvec[2])-math.atan(tag1Tvec[0]/tag1Tvec[2])))
             tag0_angle = math.acos(((tag0_cam_dist**2) + (tag_line_dist**2) - (tag1_cam_dist**2)) / (2 * tag0_cam_dist * tag_line_dist))
             tag1_angle = 180-(cam_angle+tag0_angle)
 
+            robotXCoord = (math.toDegrees(math.cos(tag_line_angle + tag0_angle))*tag0_cam_dist) + tag0Coords[0] + self.robo_space_pose[0]
+            robotYCoord = (math.toDegrees(math.sin(tag_line_angle + tag0_angle))*tag0_cam_dist) + tag0Coords[1] + self.robo_space_pose[1]
+
+            coord_array = [robotXCoord, robotYCoord]
+
             tvecZCoord = math.hypot(tag0Tvec[0], tag0Tvec[1])
 
         return coord_array
diff --git a/FRC_Fiducial_Tracking/__pycache__/Pose_Estimation.cpython-39.pyc b/FRC_Fiducial_Tracking/__pycache__/Pose_Estimation.cpython-39.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..7b513f0d4000549fb82fd7e18a0dda8fd3ea6561
GIT binary patch
literal 3189
zcmbVOOK;mo5Z+x<6!r8=j^!js4^7c9ZQ=(|AO(uRfYbC)zzve1XowaBtz~4|vZyTS
z)G{cbK=K28?xl85&Aq>(KY`bteCoZYezTNhNiN!=5_UK{JNxbI%<jzkwOX0s_s37S
zgGcj>{Z5_9pMlO6TKpJ9GRZs4rLXAlL+*0UzF^Xj=36GsH^LRNAg#B|HDpoR_?ogL
z%lH;#MON{(B!9-7+6A!Kj>9wSx!?Y3cfUpb$)7-H3oX745-^ub25c?`FpPXXVCQQU
zWD%HlR(}%3!RK)j9{Ncbbz5Zl=tn$ZZlh_;l&8#NsQ~3|?CNLgi!o<N228Qzn2KY{
z3~|h?ocL|86-7$M*}RH&qr{7Qek<^LFe0l&Dh#@bwj--(wXo|aLF^cr83&!0$^>dQ
zM-wOJy<>`k@#ps1UUV3&^};p3r@{`Fx<|10UaHpG<EPu+<4_*8LcilZSAOdt?6%iv
z>)z$w@Avwd?RjB0Ogzt`J=-9RTlkmx$;z~+?vFHE<f74!__53C=p-!VZ^%su#xwSg
z?>HheIzcz9z7FGXw-b0UG<JldwX(AG6W{Zd^80#IIjPW)b=1RPPz4|B75T~Xj77Od
zi+n%&ErTKN=n(E9plvqbu!-%%D#;JbQ?7oa-XnI(zx!n%5(IbONX07Y;BXF%q;O`X
zhJ-|lB?ivOxfOQKF;;}0J({y$!kDS&YK;AIlW;1su*#ayZ)j9PXtIHM1({DFSEV(^
ztqpOCBb-6u%Is;os{g|->=AZdu3AdXw2)eD6Y;C3{A>=G^>bX`v0d|(e1F=m0r?`|
zcWo$#FMvq?`Hwytl*SmPw3ymyNw2;zTz!F7e|7zocYhp5Q5=X<L&-dH<Xa$6<jO+{
zygHOrMs|>vj#WOUZ5zw-*iXx_zqT<L`&44ymH8vt<ouB=pI=KB)2g<xN(gC9^EKk%
zn1ebZx|Ei+tW4+aTny&4tfFO=p=>^B`~%7YW-Oy!LtB|dyOE#Qm2?7)WOOdi`Ct+H
z&2&zm@jB5Jmd?|+iF2RM;pEmEjPwUf(oPrCr4@>71G6q|#y()Pl~TGc?Of1Byr8?P
zGvNhguZsc^mk4Foi(f&URYjRX&4FasTK=I<hh7qSd5TP42Q7sv!&HehOl0akB*a%L
z$&6MM=ULIB)EdgnQqcDLiq^@DIP4B3$ErvY%kTAq?nv!KQJTwa8ODj<Z3S6juiuN3
ztP*#kUf}uNb|=UTe>cubN%Saat00Io6Dg_-ilv=pownROS?75x<DpxGG0$O#TPEAT
z*`|!2^DicNH|I-a@sluV?YTCwPoqx9EfV*_|K7C$wXU&<+ZdukrYFZhg>=Z*z<Zsr
z8+f!Km%8g7`E%Uexa`))%6gk_i4=2|FxMyejhvs&$fb6W<SSF;;s+p1n0%gB`J!<7
zL+*-eyegW!$v+0Sh>=B%Z1F8|i#K>f+!7D?U1&7KNpmKZkD+%Qi;7a$KMXuCD|_By
zB#%%vysYYZua5jqKBA~Ds0AV)5uuzvL8yW#I&gFbV+y)0Y|A#ZY^s3KxuK>D##1nP
znnB_aDGCNIsxAd66nPTN<wz$(x$!r0B*B4+SA&t7Z39=Mu%jA4&ff<s#4Up0eXu|=
zP*MH_z(e7%msxazyyM~bWccknh0H*d^f7m<m-oawp6d0ddMC3US;exSq74f@MXD(j
zNn7No(U17CPi~=;urp4tAfyIPB{4-3A&yF-egUZ^XrZWI$4A{D-oy)R<CsVvWRtkf
z*&46jhvpXAu)yp76-($I>*%b5p%)pgePF0NWEC3uS)g3%5lUEOlN4xpfH=mEc6%Fh
z8T;YuEyu`>{1Dx&Kv#Pcfe^<VXq&F+Z=h|?`t1WKYZK@sF;V3=g(BJ1>ybQ02}>(}
z3ubH|&d!;U!opGdMIR+L<>#V72^MEa@1&6ao2IGH6!xZSSirF^_oCi|K;0p7o5)QX
zFSvHp>qT*x1g`DNeLOw82~r4Ni(ax9B)(f6jeF5i;#NmG)SGp}8W6uk)7OdNBH!hT
wWKJ=+LMMI$0)3!#EVYxg(gc{ks``m%f0keFhg3LXy3sbWL;VxGXdAZu7e!3G>;M1&

literal 0
HcmV?d00001