SMUX testing and obstacle detection

Added a program that display the smux values. Edited autonomous so it
stops based off of ultra sonic read obstacle whilst going forward