Fixed bug in controller that would set stabilized state to 0. This was happening because if the state reached stopping region and already
satisfied the reachability criterion, we were not setting the stabilized state to the input state.