Ultrasonic update

This code includes the ultrasonic sensor to see if we're too close to
the wall when triangulating, useful so we dont get stuck in a loop