Further updates to everything

All subsystems done, as well as many of the commands. Wiwill need to
wait for more complete robot to test.