Big Updates!

Have tested current autonomous, teleop should be ready to roll out, and
a few bits here and there to test certain components out.