Working on a better (hopefully) 'drive forward' routine
Completed, but untested/verified version