Cleaned code a bit, added ultrasonic sensor data