What is the mosfet for? You can make a voltage divider circuit with a couple resistors if you want 3.1 volts to go to the sensor, but it'll also just use the 5 volts from the arduino just fine (see the data sheet). Try that, and see what happens. There are also much simpler ways to write your code. Was that code written for some other function that you adapted for this purpose?
This string is about a ping sensor and fading an led, but it should give you some ideas: http://arduino.cc/forum/index.php/topic,86560.0.html