#include <Servo.h>
Servo servoPin;

#define trigPin 13
#define echoPin 12
#define ledG 4
#define ledO 3
#define ledR 2

void setup() {
  servoPin.attach(11);

  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(ledG, OUTPUT);
  pinMode(ledO, OUTPUT);
  pinMode(ledR, OUTPUT);
}

void loop() {
  long duration, distance;

  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);

  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  distance = 0.3435 * (duration / 2); //convert to same value in docx

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

  if (distance <= 500) {
    digitalWrite(ledR, HIGH);
    digitalWrite(ledO, LOW);
    digitalWrite(ledG, LOW);
    servoPin.write(0);
  }
  else if (distance > 500 && distance <= 1000) {
    digitalWrite(ledR, LOW);
    digitalWrite(ledO, HIGH);
    digitalWrite(ledG, LOW);
  }
  else {
    digitalWrite(ledR, LOW);
    digitalWrite(ledO, LOW);
    digitalWrite(ledG, HIGH);
    servoPin.write(90);
  }

  delay(500);
}