Have you tried writing the measured distance to the serial interface? Maybe you should try your distance sensor before installing it on your robot. I suppose you got most of the code from an example for the distance sensor, didn't you? What sensor are you using?