Closed loop stepper on 5 axis cnc machine

[Horan31]

Hi ,i want to add servo42c driver to the stepper motors of the machine to control their position in closed loop and to reduce the over heating of the stepper motors, so i have downloaded the grbl-mega-5 firmware but now i need to know how to wire the servo42c with Arduino mega and the 1.4 ramps shield.
Also I want to know what changes to make in the grbl firmware if i want to apply PID control algorithm.
Thanks .