long readUltrasonicDistance(int triggerPin, int echoPin)
{
pinMode(triggerPin, OUTPUT); // Clear the trigger
digitalWrite(triggerPin, LOW);
delayMicroseconds(2);
// Sets the trigger pin to HIGH state for 10 microseconds
digitalWrite(triggerPin, HIGH);
delayMicroseconds(10);
digitalWrite(triggerPin, LOW);
pinMode(echoPin, INPUT);
// Reads the echo pin, and returns the sound wave travel time in microseconds
return pulseIn(echoPin, HIGH);
}
Have you verified all hardware connections? How is your circuit powered? Can you post a schematic and clear photos of your circuit?
Have you tried putting in a Serial.print after the first if-statement in the main loop to verify you get there and one before it to see what kind of distance measurements you get?
Have you tried println-ing the raw values you get through readUltrasonicDistance?
Have you tried a very simple sketch that drives the servo without any if-conditions to verify you can successfully drive it from your Arduino?
You need some temporal separation between pings or you are likely to get false echos. One rangefinder cannot distinguish its ping from an other's. I use 30 to 50 milliseconds between successive pings.