Re-structured Arduino code

Should be close to final implementation