Thanks for the responses. Unfortunately, our mentor was not at school today, so no robotics, so I still do not have code to post.
As for the printfs, they are another issue I've been looking into. I have tried the code with and with our printfs and get the same result either way. I'm not sure whether this is relevant, but when I was using them, I was unable to get them to work. I have to do a little more investigation in to them though, so I think that is a separate issue.
Thxe