Measuring position and mutual angle between two ArUco markers

edit

Hello everyone, I'm student of automation and robotics control who is interested image processing. Now I am working on my first project this is a drawing a robot controlled by vision. The robot is built on the basis of the Arduino platform. If you are interested how this looks like photo below:

To make the method of drawing as accurate as possible I used stepper motors such as in 3D printer, but I don't have any feedback information about the robot's position in the real world. A camera connected to Raspberry Pi hangs on the ceiling and control the whole workingspace. So I thought that it would be a good idea to control it using the mutual position of the two ArUco markers on the table. First will be reference mark stationary lying on the corner of the table with coordinates (x,y) x = 0, y = 0 with mm or cm not pixcels! The second one will be on top of the robot and I also need an angle beetwen them in order to control robot properly.

I need something like this, but in mm or cm not pixcels https://www.youtube.com/watch?v=njab2... - but this person didn't share source code :/

By now I have prototype of robot and program in C++ which can detect ArUco markers, draw (x,y,z) axis on them, have coordinates of their corners and also camera matrix, rotation and translation vector and distance coefficients.

Unfortunately I'm stuck in a place and I do not know how to go about it... I will be grateful for the restaurant advice and tips. Thanks in advance :)