I am looking for a way to calculate the distance between two points. I have an EOAT that has a spring-loaded probe whose position can be sensed with a proximity switch. I want to use it to measure the distance between two surfaces.

My current procedure is to skip jump to a reference surface moving along the tool's Z axis, pushing the probe end against the surface until the proximity sensor turns on. The robot stops and records the position to a Position Register using LPOS. I then offset the position about 5mm along the tool's X and then skip jump to the second surface. When the proximity sensor turns on the robot stops and records the second reference point to another position register.

I currently subtract the Z part of the coordinates to find the distance. This works great if the tool's Z axis is parallel to the World Z axis. Unfortunately, I need to measure the distance regardless of the tool's orientation. Many of the surfaces are at angles to any of the X, Y, or Z axes. Is there a way to save the position relative to the tool head? Or is there an easier way to calculate this distance? I am using tool offsets to advance the tool head is there a way to leverage this?

I am using TP on an LRMate200ic with an R-30ia controller, it is about 15 years old and does not have any advanced math functions installed.

I am currently leaning towards using the Pythagorean theorem to find the distance but with no square root function, it seems annoyingly difficult. I may resort to using Newton's approximation. I was thinking that there has to be a function to compare two points but so far my search has come up short.