Refactored Arduino C source to be cleaner. There will probably be more of this later, no likey 60+ line functions.