Implemented StateMachine Class

Implemented StateMachine Class that tracks what state we are in, how
long we have been in it, and if the timer has expired.
Moved overallState & driveState to use stateMachine.