#include <Servo.h>

const byte L1 = 2, L2 = 3, R1 = 4, R2 = 7;
const byte SERVO_PIN = 9, TRIG = 10, ECHO = 11;
Servo scanner;

void drive(int left, int right) {
  digitalWrite(L1, left > 0);  digitalWrite(L2, left < 0);
  digitalWrite(R1, right > 0); digitalWrite(R2, right < 0);
}

long distanceCm() {
  digitalWrite(TRIG, LOW); delayMicroseconds(2);
  digitalWrite(TRIG, HIGH); delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  unsigned long us = pulseIn(ECHO, HIGH, 25000UL);
  return us == 0 ? 0 : us / 58;
}

long lookAt(int angle) {
  scanner.write(angle);
  delay(400);
  return distanceCm();
}

void setup() {
  pinMode(L1, OUTPUT); pinMode(L2, OUTPUT);
  pinMode(R1, OUTPUT); pinMode(R2, OUTPUT);
  pinMode(TRIG, OUTPUT); pinMode(ECHO, INPUT);
  drive(0, 0);
  scanner.attach(SERVO_PIN);
  scanner.write(90);
  delay(500);
}

void loop() {
  long ahead = distanceCm();
  if (ahead == 0) { drive(0, 0); delay(150); return; }
  if (ahead > 25) { drive(1, 1); delay(80); return; }

  drive(0, 0); delay(100);
  drive(-1, -1); delay(300);
  drive(0, 0);
  long left = lookAt(150);
  long right = lookAt(30);
  scanner.write(90); delay(300);

  if (left == 0 && right == 0) {
    drive(0, 0); delay(200); return;
  }
  if (left > right) drive(-1, 1);
  else drive(1, -1);
  delay(450);
  drive(0, 0);
}