Some structure and utils

Added last year's utilities.
Structured code a bit
More structure

-Moved constants and created Ports class in new Config package
-Added rest of robot systems:shooter, intake