Sounds much like:
I'd code it with a Finite State Machine as in
... but add a HOMING state:
Graphviz online