@jp31310: I did not go into detail with your code, but I think we are in the same line of thought: I have defined "quadrants" and based on the "quadrant" different logic takes place.
You can find my code here: GitHub - drazha/Son-Dad-Rover-Mk-I: 1st Rover design and project based on Arduino UNO R3 controller and off-the-shelf-components
It is a bit messy and requires refactoring, but the general idea is there...