I am trying to write a routine that creates a TCP based on the robots position relative to a fixed reference point. I got the program to work creating a new TCP on the spot where the reference point is, but it only works if R=0 relative to the user frame. Once I rotate the tool and attempt to run the routine again, the TCP will deviate from the reference point. I'm assuming there is something I missed in my math involving trigonometry, but I cant figure it out. The code isn't very efficient because I wanted registers at each step so I could check each calculation. If anyone has any suggestions, I could really use the help.
In this case, the new TCP would be created on the corner of the part where it touches the red indicator.
1: UTOOL_NUM=1 ;
2: UFRAME_NUM=5 ;
3: PR[GP1:12:Grip Tool Frame]=UTOOL[1:ToolCurrentGripper] ;
4: R[30]=PR[GP1:12,1:Grip Tool Frame] ;
5: R[31]=PR[GP1:12,2:Grip Tool Frame] ;
6: R[32]=PR[GP1:12,3:Grip Tool Frame] ;
7: ;
8: PR[GP1:10:CURRENT LPOS]=LPOS ;
9: R[34]=PR[GP1:10,1:CURRENT LPOS] ;
10: R[35]=PR[GP1:10,2:CURRENT LPOS] ;
11: R[36]=PR[GP1:10,3:CURRENT LPOS] ;
12: ;
13: R[37]=PR[GP1:15,1:Gate Reference] ;
14: R[38]=PR[GP1:15,2:Gate Reference] ;
15: R[39]=PR[GP1:15,3:Gate Reference] ;
16: ;
17: R[40]=R[34]-R[37] ;
18: R[41]=R[35]-R[38] ;
19: R[42]=R[36]-R[39] ;
20: ;
21: ;
22: R[43]=0-R[40]-R[30] ;
23: R[44]=0+R[41]+R[31] ;
24: R[45]=0+R[42]+R[32] ;
25: ;
26: PR[14,1:Gate Compensated]=(0+R[43]) ;
27: PR[14,2:Gate Compensated]=(0+R[44]) ;
28: PR[14,3:Gate Compensated]=0+R[45] ;
29: UTOOL[2:ToolCurrentGate]=PR[GP1:14:Gate Compensated] ;
30: UTOOL_NUM=2 ;