Changed write multiple registers function

As suggested by mbenedetti in the issues section..