Thanks, I fixed the code with your suggestion. But the measurement was still not accurate. I added +6 to the end of the line because the result was on average 6-7 cm less than the actual distance. I'm thinking this is because the voltage on USB port must be less than 5V? If I had 5V of external reference on AREF pin, I think the results would be pretty much accurate.
Now...to test other sensors 
Do you know maybe of any good Atmel tutorials so I could write in C and manipulate registers and libraries? I was reading on the forums about how to get Arduino Mega to work with Atmel Studio 6 because I want a software debuger, because it makes programing so much easier.