Misschien helpt het om mijn code eens te proberen. Ik heb het anders aangepakt. Je kan er over lezen in deze forum post:
http://arduino.cc/forum/index.php/topic,93480.0.html