Implement collision detection function

[CraigBryan]

A function that uses the sonar sensor and returns whether or not a collision is going to happen. It must be fast, as it will be called in between wheel sensor readings.