روبوت تفادي العوائق

#include <Servo.h>

#define TRIG 9
#define ECHO 10
#define IN1 4
#define IN2 5
#define IN3 6
#define IN4 7

Servo scanner;

long readDistanceCm() {
  digitalWrite(TRIG, LOW);  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH); delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  long duration = pulseIn(ECHO, HIGH, 30000);
  if (duration == 0) return 400;
  return duration * 0.034 / 2;
}

void forward()  { digitalWrite(IN1,HIGH); digitalWrite(IN2,LOW);  digitalWrite(IN3,HIGH); digitalWrite(IN4,LOW); }
void backward() { digitalWrite(IN1,LOW);  digitalWrite(IN2,HIGH); digitalWrite(IN3,LOW);  digitalWrite(IN4,HIGH); }
void turnLeft() { digitalWrite(IN1,LOW);  digitalWrite(IN2,HIGH); digitalWrite(IN3,HIGH); digitalWrite(IN4,LOW); }
void turnRight(){ digitalWrite(IN1,HIGH); digitalWrite(IN2,LOW);  digitalWrite(IN3,LOW);  digitalWrite(IN4,HIGH); }
void stopAll()  { digitalWrite(IN1,LOW);  digitalWrite(IN2,LOW);  digitalWrite(IN3,LOW);  digitalWrite(IN4,LOW); }

void setup() {
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
  pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT);
  scanner.attach(11);
  scanner.write(90);
  delay(500);
}

void loop() {
  long d = readDistanceCm();

  if (d > 20) {
    forward();
  } else {
    stopAll();
    delay(200);
    backward();
    delay(350);
    stopAll();

    scanner.write(150); delay(500);
    long left = readDistanceCm();
    scanner.write(30);  delay(500);
    long right = readDistanceCm();
    scanner.write(90);  delay(300);

    if (left >= right) turnLeft(); else turnRight();
    delay(400);
    stopAll();
  }
  delay(60);
}