Updated controller, state machine still needs fizing