A basic ultrasound algorithm for Arduino. To work with the HC-SR04 ultrasonic sensor and a servo motor.

UltrasoundAlgorithm

#include <Servo.h>
#define trig 6
#define echo 5
#define motor 3
int distance;
Servo servoMain; // Define our Servo

int ultrasound(){
  int i, temp, distance = 0;  
  for(i=0; i<20; ++i) {
    delay(2);
     digitalWrite(trig, HIGH);
    delayMicroseconds(10);
    digitalWrite(trig, LOW);
    temp = pulseIn(echo, HIGH)/58;
    if(temp > 500) return 0;
    distance += temp;
    temp = 0;
  }
  distance = distance/20;
    return distance;
}

void setup(){
  servoMain.attach(10); // servo on digital pin 10
  pinMode(motor, OUTPUT);
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
  Serial.begin(9600);
}

void loop(){  
 delay(100);
 Serial.println(ultrasound());
 ultrasound();
 if ((ultrasound()) < 50)
  servoMain.write(0);
 else
  servoMain.write(180);
}