Das wird so nichts.
Wenn Du ein 4x4 hast, musst Du eine Taste als "Ende einlesen" definieren - wann sonst wird denn festgestellt, ob da nur 1 oder 111 gewollt ist?
Also musst Du die Einleseroutine komplett neu bauen und das ungefähr als Ansatz(!) so:
id = readnumber();
byte readNumber()
{
byte code = 0;
if (nicht auf # gedrückt || code > 255 || weitere Abbruchfunktion)
{
// Hier gehört eine Abbruchfunktion nach Zeit rein!
/* Taste 0-9 erkannt?
Zähle rauf
Warte auf die nächste Stelle oder Ende
*/
}
return code;
}
Leider verschweigst Du, was Du verwendest. sowas geht auch einfacher.