Moved printMap to .cpp and changed return type of driveDistance
Added a lot of code to state machine

Also changed some constants after switching to a new Arduino shield