If errcount > 5 set wrModeOn to False.

This modifications are needed because each time we reach an errcount > 5 (t1 timestamp flagged as
incorrect 5 consecutive times), we get stucked on uninitialized servo state.