Cleaned up Arduino code (a bit...)