Switch from pi, if large error to disturbance mode control if small error