Pololu maze solving FSM