motionDetector-servo

#include <Servo.h>
#define servoRP 2
#define servoLP 3
#define trigPin 13
#define echoPin 12
#define ERR_MARGIN 5

long average = 0;
long duration;
long distance[2];
int intruder = 0;
Servo servoR;
Servo servoL;

void setup()
{
  Serial.begin(9600);
  servoR.attach(servoRP);
  servoL.attach(servoLP);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(servoRP, OUTPUT);
  pinMode(servoLP, OUTPUT);
  servoR.write(1);
  servoL.write(1);

  for (int i = 0; i < 500; i++)
  {
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    duration = pulseIn(echoPin, HIGH);
    average += (duration/2) / 29.1;
    Serial.print(duration);
    Serial.print(", ");
    Serial.println((duration/2) / 29.1);
    delay(80);
  }
  average /= 500;
  Serial.print("Average: ");
  Serial.println(average);
}

void loop() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance[0] = distance[1];
  distance[1] = (duration/2) / 29.1;

  if ((distance[0] < average - ERR_MARGIN) && (distance[1] < average - ERR_MARGIN))
  {
    Serial.println(distance[0]);
    Serial.println(distance[1]);
    Serial.println();
    intruder = 1;
  }

  //Serial.print(distance);
  //Serial.println(" cm");

  if (intruder > 0)
  {
    intruder--;
    //analogWrite(buzzer, 100);
    servoR.write(90);
    servoL.write(90);
    delay(500);
    servoR.write(1);
    servoL.write(1);
    delay(1000);


  }
  else
  {
    delay(80);
    //analogWrite(buzzer, 0);
  }
}