Sorry, but I have no idea in which bit of your code you're having problems, and I'm not particularly inclined to play 20 questions. If you can strip your code down to the bare essentials, and describe your difficulties in adequate detail, I'm sure you can get an answer.

...roboticus

When your only tool is a hammer, all problems look like your thumb.