Управление серводвигателем с матричной клавиатуры

Друзья, столкнулся с проблемой управления сервой с клавиатуры. Использовал стандартный скетч опроса кнопок с клавиатуры. Для примера хотел сделать управление 2 сервами с помощью 4 кнопок: при удержании кнопки 1 -изменение позиции сервы с шагом +1 градус относительно начального положения ; удержание кнопки 4 - изменение позиции сервы с шагом -1 градус относительно начального положения; кнопки 7 и 8 отвечают за тоже самое только второй сервы. Вот какой уже день бьюсь, не могу дельного ничего придумать. Уверен, что дело в коде...
Сам скетч:

Код (C++):

#include <Multiservo.h>
Multiservo servo0;
Multiservo servo1;

void setup() {
pinMode (13, OUTPUT);
  pinMode (14, OUTPUT);
  pinMode (15, OUTPUT);
  pinMode (16, INPUT); // инициализируем порты на вход с подтяжкой к плюсу (принимают нули на строках)
  digitalWrite(16, HIGH);
  pinMode (17, INPUT);
  digitalWrite(17, HIGH);
  pinMode (18, INPUT);
  digitalWrite(18, HIGH);
  pinMode (19, INPUT);
  digitalWrite(19, HIGH);
  Serial.begin(9600); // открываем Serial порт

  servo0.attach(0);
  servo1.attach(1);

}

void matrix(){
digitalWrite(15,LOW); // устанавливаем ноль на 15 выходе
if(digitalRead(15)==LOW){// проверяем
digitalWrite(14,HIGH);// ставим логическую единицу на выходы 14 и 13, чтобы снимать показания с столба кнопок (1,4,7,*)
digitalWrite(13,HIGH);//

if(digitalRead(19)==LOW){// проверяем есть ли ноль на 19 входе
  Serial.println('1');// выводим в порт номер кнопки
  int pos=servo0.read();//считываем положение сервы и присваиваем переменной pos
  int pos1=pos+1;//увеличиваем значение pos на единицу
  servo0.write(pos1); // говорим серве занять эту позицию
  delay(300);
  }
else if(digitalRead(18)==LOW){

  Serial.println('4');
  // выводим в порт номер кнопки
  int pos=servo0.read();//считываем положение сервы и присваиваем переменной pos
  int pos1=pos-1;//уменьшаем значение pos на единицу
  servo0.write(pos1); // говорим серве занять эту позицию
  delay(300);
}
else if(digitalRead(17)==LOW){
  Serial.println('*');
  // выводим в порт номер кнопки
  int pos=servo1.read();//считываем положение сервы и присваиваем переменной pos
  int pos1=pos-1;//уменьшаем значение pos на единицу
  servo1.write(pos1); // говорим серве занять эту позицию
  delay(300);
}
else if(digitalRead(16)==LOW){
Serial.println('7');// выводим в порт номер кнопки
  int pos=servo1.read();//считываем положение сервы и присваиваем переменной pos
  int pos1=pos+1;//увеличиваем значение pos на единицу
  servo1.write(pos1); // говорим серве занять эту позицию
  delay(300);
}
}
}

void loop() {
  matrix();
  }

Есть еще один скетч. Там для опроса клавиатуры используется библиотека. В этом скетче управлять сервой можно так: один раз нажал на кнопку - серва изменила положение на 1 градус. А нужно управлять так: пока кнопка нажата и удерживается серводвигатель меняет положение, как кнопка отпущена серво останавливается.

Код (C++):

#include <Keypad.h>
#include <Multiservo.h>
Multiservo servo0;
Multiservo servo1;


const byte ROWS = 4;
const byte COLS = 3;
char keys[ROWS][COLS] = {
  {'1','2','3'},
  {'4','5','6'},
  {'7','8','9'},
  {'#','0','*'}
};
byte rowPins[ROWS] = {19, 18, 17, 16};
byte colPins[COLS] = {15, 14, 13};
Keypad keypad = Keypad( makeKeymap(keys), rowPins, colPins, ROWS, COLS );
void setup()
{
  Serial.begin(9600);
  servo0.attach(0);
  servo1.attach(1);
 

}
void loop()
{
  char key = keypad.getKey();
  if (key == '1'){
          Serial.println(key);
          int pos=servo0.read();
          int pos1=pos+1;
          servo0.write(pos1);
          delay(200);
  }
  else if(key=='2')
  {
    Serial.println(key);
          int pos=servo0.read();
          int pos1=pos-1;
          servo0.write(pos1);
          delay(200);
  }
  else if (key == '4'){
          Serial.println(key);
          int pos=servo1.read();
          int pos1=pos+1;
          servo1.write(pos1);
          delay(200);
  }
  else if(key=='5')
  {
    Serial.println(key);
          int pos=servo1.read();
          int pos1=pos-1;
          servo1.write(pos1);
          delay(200);
  }
    }
  }

 

в библиотеке Keypad удержания кнопки нет
придумывай свою

 

Уже придумал. В первом скетче сигнал постоянный, однако серва не управляется. По всей видимости кривой код у меня.

 

А во втором серва управляется, но только меняет положение с каждым нажатием кнопки, т.е дискретно! Замкнутый круг какой то!