lato "servo" non posso aiutarti, non ho mai usato dei servo e quindi non so come si pilotino con l'Arduino.
Sul resto ti consiglio di fare delle prove.
Anche mettere un po' di Serial.print di debug non è male, per stampare i vari punti del programma dove passa l'elaborazione ed i valori delle variabili per capire che strada prende il flusso del programma.