#include <Servo.h>
Servo myServo;
long duration;
int distance;
void setup() {
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  Serial.begin(9600);
  pinMode(9, OUTPUT);
  pinMode(10, INPUT);
  myServo.attach(3);
  myServo.write(90);
}

void loop() {
  digitalWrite(9, LOW);
  delayMicroseconds(2);

  digitalWrite(9, HIGH);
  delayMicroseconds(10);
  digitalWrite(9, LOW);
  duration = pulseIn(10, HIGH);
  distance = duration * 0.034 / 2;
  Serial.println(distance);

  if(distance > 40){
    digitalWrite(5, LOW);
    digitalWrite(6, HIGH);
    digitalWrite(7, LOW);
  }else if(distance >= 20){
    digitalWrite(5, HIGH);
    digitalWrite(6, LOW);
    digitalWrite(7, LOW);
  }else{
    digitalWrite(5, LOW);
    digitalWrite(6, LOW);
    digitalWrite(7, HIGH); 
  }
  delay(100);
}