Made the terrible code slightly less terrible. Experimented with servo range issue.