Servo Enstops working!

Added servo actuated endstop coding to allow G28 command to engage and retract a servo to specified angles.
Also added code to servo_init command to initialize servo to retracted position.
Integrated fixes from: https://github.com/ErikZalm/Marlin/pull/504