Added mode in autonomous for testing ultrasonic sensor, removed unnessecary camera code form main class