Updated error capture + added reset

Corrected Error statments to be valid, and not drop to idle! Ensured
that after >5 Errors, board is reset via AVR Watchdog.