Replace digitalWrite() with register writes - fixed interrupt conflicts