#define trigPin 9
#define echoPin 10
#define leftServo 3
#define rightServo 5
#define ledMove 7
#define ledStop 8

long duration;
int distance;

void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(ledMove, OUTPUT);
  pinMode(ledStop, OUTPUT);
  analogWrite(leftServo, 0);
  analogWrite(rightServo, 0);
}

int getDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  return duration * 0.034 / 2;
}

void moveForward() {
  analogWrite(leftServo, 200);
  analogWrite(rightServo, 200);
  digitalWrite(ledMove, HIGH);
  digitalWrite(ledStop, LOW);
}

void stopRobot() {
  analogWrite(leftServo, 0);
  analogWrite(rightServo, 0);
  digitalWrite(ledMove, LOW);
  digitalWrite(ledStop, HIGH);
}

void turnRight() {
  analogWrite(leftServo, 200);
  analogWrite(rightServo, 0);
  delay(500);
}

void loop() {
  distance = getDistance();
  if (distance <= 20) {
    stopRobot();
    delay(300);
    turnRight();
  } else {
    moveForward();
  }
}