Klingt für mich so als ob der Servo kurzzeitig sehr viel Strom zieht, die Spannung einbrechen lässt und der Mikrocontroller einen Reset auslöst.
Hast du die zusätzlichen Kondensatoren, wie im Schaltplan des RP6 eingezeichnet, schon bestückt? Das wären C26, C27 und C28.