Arduino code - Integers mixed up