Updated program

We updated the program to work on our current robot. We have tested all
the commands and they all work thus far.