The Test mode works
Add function to write to any address

Also remove the sleeps, which aren't required, and also try to put the
functions in class functions.