Scanning obstacle rover on an Arduino Uno

// Scanning obstacle rover: an ultrasonic eye on a servo, so it can look around.
//
//   HC-SR04 on the servo: TRIG pin 12, ECHO pin 11 (VCC to 5V, GND to ground)
//   Servo (the neck): signal pin 10, 90 degrees is straight ahead, 180 is left
//   TB6612: PWMA 5, AIN1 2, AIN2 4 (left)   PWMB 6, BIN1 7, BIN2 8 (right)   STBY 9
//
// A fixed sensor can only say "something is ahead". Put it on a servo and the
// robot can ask which way is clear, which turns a dumb bumper into a decision.
//
// # The loop
//
//   cruise   drive forward, pinging straight ahead every 70 ms
//   blocked  something closer than STOP_CM: stop
//   scan     turn the head to the right, ahead and left, and ping each way
//            (three pings, the middle one kept, so one bad echo cannot fool it)
//   choose   the direction with the longest distance wins
//   turn     spin toward it by about the angle the head was turned, and go
//
// If even the best way is closer than TRAPPED_CM the robot is in a corner and
// it turns right around instead.
//
// # Turning by angle
//
// A turn is timed, not measured. A standing spin at this speed turns about 0.39
// degrees every millisecond, so 2.6 ms buys a degree. A wheel encoder would
// measure the turn instead (see the wheel-encoder project).

#include <Servo.h>
#include <SparkFun_TB6612.h>

const int TRIG = 12;
const int ECHO = 11;
const int NECK = 10;

const int AIN1 = 2;
const int AIN2 = 4;
const int PWMA = 5;
const int BIN1 = 7;
const int BIN2 = 8;
const int PWMB = 6;
const int STBY = 9;

const int STOP_CM = 25;
const int TRAPPED_CM = 20;
const int CRUISE = 170;
const int SPIN = 170;

const int ANGLES[3] = {20, 90, 160};
const char *NAMES[3] = {"right", "ahead", "left"};

Motor leftMotor = Motor(AIN1, AIN2, PWMA, 1, STBY);
Motor rightMotor = Motor(BIN1, BIN2, PWMB, 1, STBY);
Servo neck;

unsigned long lastPing = 0;

// Distance in centimeters, 999 when nothing answers.
int ping() {
  digitalWrite(TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  unsigned long echo = pulseIn(ECHO, HIGH, 12000);
  if (echo == 0) return 999;
  return echo / 58;
}

// The middle of three pings: one stray echo cannot change the answer.
int steadyPing() {
  int a = ping();
  delay(70);
  int b = ping();
  delay(70);
  int c = ping();
  if ((a >= b && a <= c) || (a <= b && a >= c)) return a;
  if ((b >= a && b <= c) || (b <= a && b >= c)) return b;
  return c;
}

void drive(int left, int right) {
  leftMotor.drive(left);
  rightMotor.drive(right);
}

// Positive degrees turn left (counterclockwise), negative turn right.
void turnBy(int degrees) {
  if (degrees == 0) return;
  int direction = degrees > 0 ? -1 : 1;
  int size = degrees > 0 ? degrees : -degrees;
  drive(0, 0);
  delay(150);
  drive(SPIN * direction, -SPIN * direction);
  delay((long)size * 13 / 5);
  drive(0, 0);
  delay(150);
}

void scanAndChoose() {
  int best = -1;
  int bestCm = -1;
  int seen[3];
  for (int i = 0; i < 3; i++) {
    neck.write(ANGLES[i]);
    delay(450);
    seen[i] = steadyPing();
    if (seen[i] > bestCm) {
      bestCm = seen[i];
      best = i;
    }
  }
  neck.write(90);

  Serial.print("scan: right ");
  Serial.print(seen[0]);
  Serial.print(" cm, ahead ");
  Serial.print(seen[1]);
  Serial.print(" cm, left ");
  Serial.print(seen[2]);
  Serial.println(" cm");

  if (bestCm < TRAPPED_CM) {
    Serial.println("boxed in: turning around");
    turnBy(180);
  } else {
    Serial.print("clearest way: ");
    Serial.println(NAMES[best]);
    turnBy(ANGLES[best] - 90);
  }
}

void setup() {
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
  neck.attach(NECK);
  neck.write(90);
  Serial.begin(115200);
  Serial.println("Mokxi Arduino Uno: scanning obstacle rover");
  Serial.println("ready");
  delay(600);
}

void loop() {
  if (millis() - lastPing < 70) return;
  lastPing = millis();
  int ahead = ping();
  if (ahead < STOP_CM) {
    drive(0, 0);
    Serial.print("blocked at ");
    Serial.print(ahead);
    Serial.println(" cm: looking around");
    scanAndChoose();
  } else {
    drive(CRUISE, CRUISE);
  }
}