#include <Servo.h>
#define echo 11
#define trig 12
#define servoPin 5

const int default_angle = 90; 
const float speed = 0.0343;   
const int threshold = 20;   
long duration, distance;

Servo servo;

void setup() {
  Serial.begin(9600);
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
  servo.attach(servoPin);
  servo.write(default_angle);
}

void loop() {
  servo.write(default_angle);
  delay(500); 
  distance = cal_distance();
  if (distance < threshold) {
    look_left();
    look_right();
  } else {
    servo.write(default_angle);
  }
}

int cal_distance() {
  digitalWrite(trig, LOW);  delayMicroseconds(2);
  digitalWrite(trig, HIGH); delayMicroseconds(10);
  digitalWrite(trig, LOW);

  duration = pulseIn(echo, HIGH);
  if (duration == 0) {return 400;}
  distance = speed * duration / 2;
  return constrain(distance, 2, 400); 
}

int look_left() {
  servo.write(0);
  delay(500);     
  int L = cal_distance();
  Serial.print("Left: ");
  Serial.println(L);
  return L;
}

int look_right() {
  servo.write(179); 
  delay(500);       
  int R = cal_distance();
  Serial.print("Right: ");
  Serial.println(R);
  return R;
}