So I got ABL installed.

Downloaded the stable firmware from github (https://github.com/MarlinFirmware/Marlin) and modified the heck out of it following some guides here and there.

I've also done the offsets calibration and checked the hardware. Still I have two problems which I believe are related.

1. The probe switch is hitting the bed because the arm is retracting before the Z axis raises.

2. I get inconsistent Z readings when running multiple G29

Code:
echo:SD card ok>>> g28
SENDING:G28
>>> g29
SENDING:G29
Bed x: 20.00 y: 20.00 z: 6.54
Bed x: 220.00 y: 20.00 z: 6.15
Bed x: 220.00 y: 220.00 z: 6.31
Bed x: 20.00 y: 220.00 z: 6.73
Eqn coefficients: a: -0.00 b: 0.00 d: 6.57
planeNormal x: 0.00 y: -0.00 z: 1.00
echo:endstops hit:  Z:6.73
>>> g29
SENDING:G29
Bed x: 20.00 y: 20.00 z: 6.49
Bed x: 220.00 y: 20.00 z: 6.36
Bed x: 220.00 y: 220.00 z: 6.35
Bed x: 20.00 y: 220.00 z: 6.69
Eqn coefficients: a: -0.00 b: 0.00 d: 6.56
planeNormal x: 0.00 y: -0.00 z: 1.00
echo:endstops hit:  Z:6.69
>>> g29
SENDING:G29
Bed x: 20.00 y: 20.00 z: 6.65
Bed x: 220.00 y: 20.00 z: 6.27
Bed x: 220.00 y: 220.00 z: 6.27
Bed x: 20.00 y: 220.00 z: 6.34
Eqn coefficients: a: -0.00 b: -0.00 d: 6.61
planeNormal x: 0.00 y: 0.00 z: 1.00
echo:endstops hit:  Z:6.34
>>> g29
SENDING:G29
Bed x: 20.00 y: 20.00 z: 6.20
Bed x: 220.00 y: 20.00 z: 6.25
Bed x: 220.00 y: 220.00 z: 5.98
Bed x: 20.00 y: 220.00 z: 6.30
Eqn coefficients: a: -0.00 b: -0.00 d: 6.31
planeNormal x: 0.00 y: 0.00 z: 1.00
echo:endstops hit:  Z:6.30
SENDING:G28
>>> g29
SENDING:G29
Bed x: 20.00 y: 20.00 z: 6.54
Bed x: 220.00 y: 20.00 z: 6.15
Bed x: 220.00 y: 220.00 z: 6.31
Bed x: 20.00 y: 220.00 z: 6.73
Eqn coefficients: a: -0.00 b: 0.00 d: 6.57
planeNormal x: 0.00 y: -0.00 z: 1.00
echo:endstops hit:  Z:6.73
>>> g29
SENDING:G29
Bed x: 20.00 y: 20.00 z: 6.49
Bed x: 220.00 y: 20.00 z: 6.36
Bed x: 220.00 y: 220.00 z: 6.35
Bed x: 20.00 y: 220.00 z: 6.69
Eqn coefficients: a: -0.00 b: 0.00 d: 6.56
planeNormal x: 0.00 y: -0.00 z: 1.00
echo:endstops hit:  Z:6.69
>>> g29
SENDING:G29
Bed x: 20.00 y: 20.00 z: 6.65
Bed x: 220.00 y: 20.00 z: 6.27
Bed x: 220.00 y: 220.00 z: 6.27
Bed x: 20.00 y: 220.00 z: 6.34
Eqn coefficients: a: -0.00 b: -0.00 d: 6.61
planeNormal x: 0.00 y: 0.00 z: 1.00
echo:endstops hit:  Z:6.34
>>> g29
SENDING:G29
Bed x: 20.00 y: 20.00 z: 6.20
Bed x: 220.00 y: 20.00 z: 6.25
Bed x: 220.00 y: 220.00 z: 5.98
Bed x: 20.00 y: 220.00 z: 6.30
Eqn coefficients: a: -0.00 b: -0.00 d: 6.31
planeNormal x: 0.00 y: 0.00 z: 1.00
echo:endstops hit:  Z:6.30
Does anyone know if I should migrate to the development version of the firmware. By what I was able to gather the "enhanced G29 command" was merged to the development branch around March so it's not part of what I have today.