Changed algorithm for arduino