Added lots of code for future use in robot