Sehr löblich! Aber verstehst Du auch die servo.h, die Du ja einsetzt?
Ein spontanes Umschalten der Servobewegung kann man auch unterbinden, ohne gleich den ganzen Sketch zu blockieren. Aber wenn es so für dich passt, ist ja alles ok - nur warum fragst Du dann nach Optimierung?