Grove Ultrasonic not working properly?

You can use what you have now. Just add a delay after (or before) you read the sensor.