#include <Servo.h>
Servo s1;  Servo s2;  Servo s3;  Servo s4;

const int trig = 4,     echo = 3;
const int select = 2;
const int horz1 = A0,   vert1 = A1;
const int horz2 = A2,   vert2 = A3;
int       yVal1 =  0,   xVal1 =  0;
int       yVal2 =  0,   xVal2 =  0;

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

void setup() {
  pinMode(select, INPUT_PULLUP);
  pinMode(vert1, INPUT);  pinMode(horz1, INPUT);
  pinMode(vert2, INPUT);  pinMode(horz2, INPUT);
  pinMode(trig, OUTPUT);  pinMode(echo, INPUT);
  s1.attach(12);  s2.attach(11);
  s3.attach(10);  s4.attach(9);
}
void loop() {
  int selectVal = digitalRead(select);
  if (selectVal == 1) joystick_control();
  else automated();
}

void joystick_control(){
  int ang1, ang2, ang3, ang4;
  xVal1  = constrain(analogRead(horz1), 100, 900);   
  yVal1  = constrain(analogRead(vert1), 100, 900);
  xVal2  = constrain(analogRead(horz2), 100, 900);   
  yVal2  = constrain(analogRead(vert2), 100, 900);
  ang1 = map(xVal1,  100, 900, 0, 180);
  ang2 = map(yVal1,  100, 900, 0, 180);  
  ang3 = map(xVal2,  100, 900, 0, 180);
  ang4 = map(yVal2,  100, 900, 0, 180);
  s1.write(ang1);  s2.write(ang2);
  s3.write(ang3);  s4.write(ang4);
}

void automated(){
  s1.write(default_angle);
  s2.write(default_angle);
  s3.write(default_angle);
  s4.write(default_angle);
  delay(500); 
  distance = cal_distance();
  if (distance < threshold) {
    look_left();
    look_right();
  } else {
      s1.write(default_angle);
      s2.write(default_angle);
      s3.write(default_angle);
      s4.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() {
  s1.write(0);  s2.write(0);
  s3.write(0);  s4.write(0);
  delay(500);     
  int L = cal_distance(); return L;
}

int look_right() {
  s1.write(180);  s2.write(180);
  s3.write(180);  s4.write(180);
  delay(500);       
  int R = cal_distance(); return R;
}