Add servo testcode