Hab keine Zeit mir das genauer anzuschauen aber bei der Fehlersuche ist es immer hilfreich einfach Textausgaben einzubauen - also in dem Bereich wo Du den Fehler vermutest einfach nach jeder Zeile writeString_P("1 \n"); (und 2, 3, 4 usw. ) hinschreiben und die Ausgaben beobachten dann siehst Du genauer wo das Program hängen bleibt.
Variablenwerte mit writeInteger ausgeben hilft auch manchmal.

MfG,
SlyD