Maze-solving robot on an Arduino Uno

// Maze-solving robot: the left-hand rule, with three Sharp distance sensors.
//
//   GP2Y0A21 left VO on A0, front on A1, right on A2 (VCC to 5V, GND to ground)
//   DRV8833: AIN1 pin 5, AIN2 pin 6 (left motor)   BIN1 pin 9, BIN2 pin 10 (right motor)
//
// # The left-hand rule
//
// Put a hand on the left wall and keep walking, and in any maze whose walls are
// all joined to the outside you will walk every corridor and come out. The robot
// does the same with a short list of rules, checked in this order every time:
//
//   1. an opening on the left: go forward until the wheels are past the
//      corner, turn left, and drive into the new corridor
//   2. the front is clear: drive on, steering to stay centered
//   3. an opening on the right: turn right
//   4. otherwise it is a dead end: turn around
//
// # Reading a Sharp sensor
//
// It does not give a distance. It gives a voltage that falls as the wall gets
// farther away, roughly 33.9 / (distance in cm + 4.74), so the sketch turns the
// voltage back into centimeters with the same curve. Closer than 10 cm the
// output drops again, which reads a very near wall as a far one. That is why the
// maze corridors in this project are wider than that.
//
// # Staying centered
//
// With a wall on each side, the difference between the left and right distances
// is how far the robot is off center. Each wheel speeds up or slows down in
// proportion to it, the simplest kind of feedback.
//
// # Timed turns
//
// A turn is a number of milliseconds of spinning on the spot, tuned for this
// chassis. A real robot would count wheel pulses instead, which a slot sensor
// on each wheel makes easy.

const int LEFT_EYE = A0;
const int FRONT_EYE = A1;
const int RIGHT_EYE = A2;

const int AIN1 = 5;
const int AIN2 = 6;
const int BIN1 = 9;
const int BIN2 = 10;

const float OPEN_CM = 30.0;
const float BLOCKED_CM = 15.0;
const int CRUISE = 170;
const int SPIN = 170;
const float KP = 3.0;

// Spinning on the spot from a standstill. The motors take about 150 ms to get
// up to speed, so a half turn takes less than twice the time of a quarter.
const unsigned long TURN_90_MS = 270;
const unsigned long TURN_180_MS = 500;
const unsigned long PAST_CORNER_MS = 250;
const unsigned long ENTER_MS = 350;

char path[24];
int steps = 0;

void motor(int pinA, int pinB, int speed) {
  if (speed > 255) speed = 255;
  if (speed < -255) speed = -255;
  analogWrite(pinA, speed > 0 ? speed : 0);
  analogWrite(pinB, speed < 0 ? -speed : 0);
}

void drive(int left, int right) {
  motor(AIN1, AIN2, left);
  motor(BIN1, BIN2, right);
}

float distanceCm(int pin) {
  float volts = analogRead(pin) * 5.0 / 1023.0;
  if (volts < 0.3) return 99.0;
  return 33.9 / volts - 4.74;
}

void note(char move) {
  if (steps < 23) {
    path[steps++] = move;
    path[steps] = 0;
  }
  Serial.print("path: ");
  Serial.println(path);
}

// Stop, then spin on the spot for `ms`. Stopping first means every turn starts
// from rest, so the same time gives the same angle.
void turn(int direction, unsigned long ms) {
  drive(0, 0);
  delay(200);
  drive(SPIN * direction, -SPIN * direction);
  delay(ms);
  drive(0, 0);
  delay(100);
}

void setup() {
  Serial.begin(115200);
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  path[0] = 0;
  Serial.println("Mokxi Arduino Uno: maze-solving robot");
  Serial.println("ready: left hand on the wall");
  delay(500);
}

void loop() {
  float left = distanceCm(LEFT_EYE);
  float front = distanceCm(FRONT_EYE);
  float right = distanceCm(RIGHT_EYE);

  if (left > OPEN_CM) {
    Serial.println("opening on the left: turning left");
    drive(CRUISE, CRUISE);
    delay(PAST_CORNER_MS);
    turn(-1, TURN_90_MS);
    drive(CRUISE, CRUISE);
    delay(ENTER_MS);
    note('L');
    return;
  }

  if (front > BLOCKED_CM) {
    // Both walls near: steer by the difference. One wall gone: hold straight.
    float error = 0;
    if (right < OPEN_CM) error = left - right;
    int correction = (int)(KP * error);
    drive(CRUISE - correction, CRUISE + correction);
    delay(20);
    return;
  }

  if (right > OPEN_CM) {
    Serial.println("wall ahead, opening on the right: turning right");
    turn(1, TURN_90_MS);
    note('R');
  } else {
    Serial.println("dead end: turning around");
    turn(1, TURN_180_MS);
    note('U');
  }
}