#include <Servo.h>
#include <Wire.h>

Servo Servo1;

#define ledGreen 9
#define ledOrange 8
#define ledRed 7
#define trigPin 13
#define echoPin 12
#define pinServo1 6
int distanceInCm;
long duration = 0;

void setup() {
  Serial.begin(9600);
  pinMode(ledGreen, OUTPUT);
  pinMode(ledOrange, OUTPUT);
  pinMode(ledRed, OUTPUT);
  Servo1.attach(pinServo1);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

}

void loop() {
  ultraSonic();
  ledWarning();

}

void ultraSonic(){
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW); 
}

void ledWarning(){
  duration = pulseIn(echoPin, HIGH);
  distanceInCm= 0.03435 * (duration / 2);
  Serial.println(distanceInCm);
  
  if (distanceInCm > 100){
    digitalWrite(ledGreen, HIGH);
    Servo1.write(0);
  }
  if (distanceInCm > 50 && distanceInCm < 101){
    digitalWrite(ledOrange, HIGH);
    Servo1.write(0);
  }
  if (distanceInCm <= 50){
    digitalWrite(ledRed, HIGH);
    Servo1.write(90);
  }
  digitalWrite(ledGreen, LOW);
  digitalWrite(ledOrange, LOW);
  digitalWrite(ledRed, LOW);
}