Add servo write