Arduino code optimization