First working code

First code deployed to robot. - OPERATIONAL.
Added a few more actions to auton
Added console messages