Hello, i've finally get my machine syncronizing the absolute position with my servo drives Mitsubishi MR-J3

It took a lot of time but now at restarting the machine it's syncronized and squared correctly

Basically this is the working:
1- When the estop signal disapears it enable mach and waits some time to initialize the drives (my spindle tooks 2 seconds to initialize)
2 -Save the current machine position to some variable for later return to it
3- Asks the abs position to the servodrives (using a rs422 link)
4- Converts the position in units to machine units
5- Deactivate the soft limits
6- Calculate the difference between the actual position and the abs position
7- Locks the servodrives to avoid motors movement (i've got that disabling the servo limits signals)
8- Asign the slave axis to a unused axis to move it individually (because i have a gantry machine)
9- Command the software to move the distance required to go to the actual position (because you can't tell to mach4 to overwrite the machine position)
10- Unlock the servodrives to enable the motors movement
11- Move the slave axis to the corrected position (master Axis position + YOffset to square the machine)
12- Re assign the slave axis to the master Axis and enable it
13- Move the machine to the old position
Maybe would be some ways improve this, if you knows some please let me know.
I've attached the code if someone wants to try to implement it.
Thanks