Ну я бы примерно сделал так:
Описываем режим работы , это будет число от 0 до 3. Меняться оно будет по нажатию кнопки. Т.е. увеличиваться на 1 , 0, 1, 2, 3, 0 , 1 и т.д.
При очередном увеличении флаг f (логическая переменная) меняет свое состояние с фальш, на тру
В цкле loop идет проверка этого флага, и если он true, то в зависимости от режима 0-3 делается определенное действие с сервоприводом и флаг возвращается в false. По режимам 0 - поворот сервы 1 в нужное положение , 1 возврат сервы 1 в начальное положение , 2 поворот сервы 2 в нужное положение , 3 - возврат сервы в начальное положение.
Режимов можно добавлять и убавлять, работу по нажатию кнопки сделать по своей надобности. Т.к. вопрос описан крайне поверхностно то, ответил как понял.