Modified the add_servo() function to check whether the new servo has already been assigned or not.

See merge request !1