Add check to only use front sonar sensor when driving forward, and
only use rear-sensor when not. Also removed magic numbers
Merge branch 'obstacle_identification' of https://github.com/Vandborg/SW505E14-Software into obstacle_identification