Working on drive states
Still working on drive state machine
State machine should work with teleoperated joystick drive