Code slightly structured and modified
Some more comments added
Not tested on a real board in Rio yet