Ma l'hai fatto un controllo statico del consumo di memoria con avr-size oppure no?
Tanto per capire dove andiamo a cascare..
Poi hai una miriade di invocazioni ad una funzione ToSerial che altro non fa che stampare stringhe sulla seriale. Peccato che tutte quelle stringhe gliele passi dalla Ram.
Riscrivi il codice togliendo quella funzione e mettendo un semplice Serial.println in modo da poter usare le stringhe gestite in Flash con la funzione F() e racchiudendo tutto tra #define se vuoi stampare solo in caso di debug.
So che così raddoppi la lunghezza del tuo codice ma elimini un sacco di occupazione di memoria.