#define TRIG1 4
#define ECHO1 3
#define TRIG2 5
#define ECHO2 6

#include <Servo.h>
Servo S1;
int angle1;
Servo S2;
int angle2;

int distance1;
int duration1;

int distance2;
int duration2;

void setup() {
  
  pinMode(TRIG1, OUTPUT);
  pinMode(ECHO1, INPUT);
  pinMode(TRIG2, OUTPUT);
  pinMode(ECHO2, INPUT);
  pinMode(11, OUTPUT);
  S1.attach(9);
  S2.attach(10);

  Serial.begin(9600);
  

}

void loop() {

  digitalWrite(TRIG1, HIGH);
  delayMicroseconds(4);
  digitalWrite(TRIG1, LOW);

  duration1 = pulseIn(ECHO1, HIGH);
  distance1 = duration1 / 58;

  Serial.print("CM1=");
  Serial.println(distance1);
  
  if (distance1 >=20 && distance1 <=80)  {

    angle1 = map(distance1, 20,80,0,180);
    S1.write(angle1);
    delay(500);

  }   

  digitalWrite(TRIG2, HIGH);
  delayMicroseconds(4);
  digitalWrite(TRIG2, LOW);  

  duration2 = pulseIn(ECHO2, HIGH);
  distance2 = duration2 / 58;
  
  Serial.print("CM2=");
  Serial.println(distance2);

  if (distance2 >=30 && distance2 <=90) {

    angle2 = map (distance2, 30, 90, 180, 0);
    S2.write(angle2);
    delay(500);

  }

  if (distance1 <=40) {

    digitalWrite(11, LOW);
  } else if (distance1 >40 && distance1 <200) {

    digitalWrite(11, HIGH);
  }

}