Final Revision before Competition

Updated with the full code to be used at competition. Still waiting on
Autonomous testing.