#include <Servo.h>
int servo1Pin = 8;
int servo2Pin = 7;
Servo servo1;
Servo servo2;
#define trigPin 13
#define echoPin 12
#define ledR1 11
#define ledR2 11
#define ledO1 10
#define ledO2 10
#define ledG1 9
#define ledG2 9


void setup() {
  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(ledR1, OUTPUT);
  pinMode(ledR2, OUTPUT);
  pinMode(ledO1, OUTPUT);
  pinMode(ledO2, OUTPUT);
  pinMode(ledG1, OUTPUT);
  pinMode(ledG2, OUTPUT);
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
}

void loop() {
  long duration, distance;
  digitalWrite(trigPin, LOW);  
  delayMicroseconds(2); 
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10); 
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = 0.03435 * (duration / 2);

  if (distance<=100&&distance<=50){
    digitalWrite(ledR1, HIGH);
    digitalWrite(ledR2, HIGH);
    digitalWrite(ledO1, LOW);
    digitalWrite(ledO2, LOW);
    digitalWrite(ledG1, LOW);
    digitalWrite(ledG2, LOW);
    servo1.write(90);
    servo2.write(90);
  }

  else if (distance<=100&&distance>50){
    digitalWrite(ledR1, LOW);
    digitalWrite(ledR2, LOW);
    digitalWrite(ledO1, HIGH);
    digitalWrite(ledO2, HIGH);
    digitalWrite(ledG1, LOW);
    digitalWrite(ledG2, LOW);
  }
  
  else {
    digitalWrite(ledR1, LOW);
    digitalWrite(ledR2, LOW);
    digitalWrite(ledO1, LOW);
    digitalWrite(ledO2, LOW);
    digitalWrite(ledG1, HIGH);
    digitalWrite(ledG2, HIGH);
    servo1.write(0);
    servo2.write(0);
  }
  delay(500);

  Serial.print(distance);
  Serial.println(" m");
}