`YINYANG`

UF = 0

UT = 1

PR[17] = LPOS (set to Cartesian coords)

PR[17,4] = 0 (Touchup to determine Yaw)

PR[17,5] = 0 (Touchup to determine Pitch)

PR[17,6] = 0 (Touchup to determine Roll)

PR[1] = PR[17] (Initialize angles)

PR[2] = PR[17] (Initialize angles)

PR[3] = PR[17] (Initialize angles)

PR[4] = PR[17] (Initialize angles)

PR[5] = PR[17] (Initialize angles)

PR[6] = PR[17] (Initialize angles)

PR[7] = PR[17] (Initialize angles)

PR[8] = PR[17] (Initialize angles)

PR[9] = PR[17] (Initialize angles)

PR[10] = PR[17] (Initialize angles)

PR[11] = PR[17] (Initialize angles)

PR[12] = PR[17] (Initialize angles)

PR[13] = PR[17] (Initialize angles)

PR[14] = PR[17] (Initialize angles)

PR[15] = PR[17] (Initialize angles)

PR[16] = PR[17] (Initialize angles)

PR[1,1] = AR[1]

PR[1,2] = AR[2] + AR[4]

PR[1,3] = AR[3]

PR[2,1] = AR[1] + AR[4]

PR[2,2] = AR[2]

PR[2,3] = AR[3]

PR[3,1] = AR[1]

PR[3,2] = AR[2] - AR[4]

PR[3,3] = AR[3]

PR[4,1] = AR[1] - AR[4]

PR[4,2] = AR[2]

PR[4,3] = AR[3]

PR[5,1] = AR[1] - (0.5 * AR[4])

PR[5,2] = AR[2] - (0.5 * AR[4])

PR[5,3] = AR[3]

PR[6,1] = AR[1]

PR[6,2] = AR[2]

PR[6,3] = AR[3]

PR[7,1] = AR[1] + (0.5 * AR[4])

PR[7,2] = AR[2] + (0.5 * AR[4])

PR[7,3] = AR[3]

PR[8,1] = AR[1] + (0.66667 * AR[4])

PR[8,2] = AR[2]

PR[8,3] = AR[3]

PR[9,1] = AR[1] + (0.5 * AR[4])

PR[9,2] = AR[2] - (0.16667 * AR[4])

PR[9,3] = AR[3]

PR[10,1] = AR[1] + (0.33333 * AR[4])

PR[10,2] = AR[2]

PR[10,3] = AR[3]

PR[11,1] = AR[1] + (0.5 * AR[4])

PR[11,2] = AR[2] + (0.16667 * AR[4])

PR[11,3] = AR[3]

PR[12,1] = AR[1] - (0.5 * AR[4])

PR[12,2] = AR[2] + (0.16667 * AR[4])

PR[12,3] = AR[3]

PR[13,1] = AR[1] - (0.66667 * AR[4])

PR[13,2] = AR[2]

PR[13,3] = AR[3]

PR[14,1] = AR[1] - (0.5 * AR[4])

PR[14,2] = AR[2] - (0.16667 * AR[4])

PR[14,3] = AR[3]

PR[15,1] = AR[1] - (0.33333 * AR[4])

PR[15,2] = AR[2]

PR[15,3] = AR[3]

PR[16,1] = 0

PR[16,2] = 0

PR[16,3] = AR[3] + 50

J P[1] (Home)

J PR[4] Offset PR[16]

L PR[4]

C PR[1]

PR[2]

C PR[3]

PR[4]

C PR[5]

PR[6]

C PR[7]

PR[2]

L PR[7] Offset PR[16]

J PR[8] Offset PR[16]

L PR[8]

C PR[9]

PR[10]

C PR[11]

PR[8]

L PR[8] Offset PR[16]

J PR[12] Offset PR[16]

L PR[12]

C PR[13]

PR[14]

C PR[15]

PR[12]

L PR[12] Offset PR[16]

J P[1] (Home)

I think something like this would work, where you call it: YINYANG(X,Y,Z,R)