hmmm...mabye you should try with angles? take the calculation of the tilt angle from terminal.c and find what is your requested tilt angle where the robot will stop in your opinion and then do the tilt angle in relation to the distance(too far, go forward. too close, stop.).
Shouldn't be too complicated and it worked for us, but if you prefer to use your own function like that, I have no idea what the problem can be...
