Move the sonar approach code to drive.c