Added first draft of controller selection and PidController