Added servo and distance logic