Made some changes to feedback loop and reorganized controller