Completed servo control **NOT TESTED**