Add servo control functions