Need to add delay into my Auto Bed Level

I am attempting to swap over to an opto switch plunger design for my auto bed level. I have the code setup and it works with one major issue. When the servo is in the 'up" position the Z-min endstop is obviously triggered. No big deal, when I run a G28 or G29 the servo drops the plunger and the Z axis moves until the plunger trips the opto switch. Here is my issue. when I issue a G28 the servo moves to the lowered position, but marlin reads the opto before the plunger has had time to fall. I need to add a 3-5 millisecond delay between the time the servo actuates to the time marlin reads the switch value.

Any thoughts?

If you use automatic probe retraction, you could send the servo down a bit , before you do G28/G29 in start code.

Or adjust the probe parking position a bit lower, so the switch won't be triggered but the probe is above the nozzle.
-Olaf

Turns out this is the very reason for the servo disarm delay. I added 300 milliseconds to the delay and my problem is gone. Its working perfectly..