Mando RF + Keypad controlando servos

Hola.

El caso es que tengo un keypad de 4x4 y quiero hacer tal que así: si dejo pulsada una tecla te mueves hasta que la suelte.

Esto me lo hace, el problema es que no "reacciona" a tiempo cuando está moviéndose el servo y suelto el botón, a veces recibe el trigger de soltar el botón y parar el servo y otras no, y no entiendo cuál es el motivo.

Ej: Dejo pulsado el botón, empieza a moverse solo, suelto el botón, sigue moviéndose, tengo que pulsar varias veces el botón para que reciba la orden de parar.

Pongo el código del arduino receptor resumido con las funciones que interesan. Se tendría que activar cuando pulso el 7 y desactivar cuando suelto el botón.

void loop()
{
    uint8_t buf[VW_MAX_MESSAGE_LEN];
    uint8_t buflen = VW_MAX_MESSAGE_LEN;
    
    delay(5);
     
    if (vw_get_message(buf, &buflen)) // Non-blocking
    {
          
      switch(buf[0])
      {
              case '7':
                //ServoCodo
                Serial.print("He recibido un 7"); 
                botonPulsado = true;
                delay(15);                                             
                break;   
              default:
                botonPulsado = false;
                Serial.println("Boton soltado");
                break; 
       }     

       }
       //Aquí deberíamos crear una función que llame a la función de aumentar indefinidamente si la variable global está activada 
       if (botonPulsado)
       {
             Serial.println("Entró");
             pos = incPulse(servoCodo.read(), 2);
             servoCodo.write(pos);
             delay(200);
        }  
     }
 }

//Esta función incremente el valor hasta que alcanza el máximo
int incPulse(int val, int inc){
   if( val + inc  > 2000 )
	return 1000 ;
   else
	 return val + inc;
}