Bug fix: reading registers before writing

Reading registers before writing to them (otherwise the other register parts get overwritten to its default value).

Adding back the for loop that makes 5 attempts to read the register. Without it you would see a lot of failures.