Tests and refactor for feedback controller