Code Clean Up

Cleaned up lots of code, and made some minor tweaks... robot
functionality should not have changed though. There are still some
things to be done, but we need to make sure the robot still works in
order to proceed.
Update before placing matches
Update After Day 2

Everything is working just fine, with the exception that not everything
may be tuned correctly (Like turning and driving distances for
autonomous). But everything is functional
Update After Competition

Robot is fully functional.