Make sonar-based driving reasonable
Use the fixed driveToSonar function and speed things up