Refactor, plus added temperature check to servos