Robot vacuum behaviors on an Arduino Uno

// Robot vacuum behaviors: how a cheap cleaner covers a floor with no map.
//
//   DRV8833: AIN1 pin 5, AIN2 pin 6 (left motor)   BIN1 pin 9, BIN2 pin 10 (right motor)
//   Bumpers: left microswitch on pin 7, right on pin 8 (switch to ground, INPUT_PULLUP)
//   Cliff sensor: a TCRT5000 pointing at the floor, DO on pin 4
//
// # Behaviors in layers
//
// There is no map and no plan. The robot has three small behaviors, and the
// higher one always wins (this is called a subsumption architecture):
//
//   1. CLIFF   the floor stopped reflecting: back up and turn away. Highest,
//              because a fall ends the job
//   2. BUMP    a bumper pressed: back up and turn away from that side
//   3. COVER   nothing is wrong, so clean: spiral outward for the first few
//              seconds, then drive straight until something is hit
//
// Each layer only looks at its own sensors. COVER does not know about walls,
// and CLIFF does not know about covering the floor, yet together they
// cover a room.
//
// # The floor sensor
//
// A TCRT5000 looks down at the floor from a few millimeters. A floor reflects,
// so DO reads LOW. Reach a stair edge, or a black rug that swallows the light,
// and DO reads HIGH. HIGH means "no floor".
//
// # Random turns
//
// Turning by the same angle every time can leave the robot circling the same
// patch. A random angle between 60 and 150 degrees fixes that. Turns are
// timed spins: from a standstill, about 3 ms to the degree.

const int AIN1 = 5;
const int AIN2 = 6;
const int BIN1 = 9;
const int BIN2 = 10;
const int BUMP_LEFT = 7;
const int BUMP_RIGHT = 8;
const int CLIFF = 4;

const int CRUISE = 180;
const int SPIN = 170;
const unsigned long SPIRAL_MS = 5000;

enum Mode { SPIRAL, CRUISING };
Mode mode = SPIRAL;
unsigned long modeSince = 0;

void motor(int a, int b, int speed) {
  if (speed >= 0) {
    analogWrite(b, 0);
    analogWrite(a, speed);
  } else {
    analogWrite(a, 0);
    analogWrite(b, -speed);
  }
}

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

// Positive turns left, negative turns right, in degrees.
void turn(int degrees) {
  int direction = degrees > 0 ? -1 : 1;
  long size = degrees > 0 ? degrees : -degrees;
  drive(0, 0);
  delay(150);
  drive(SPIN * direction, -SPIN * direction);
  delay(size * 3);
  drive(0, 0);
  delay(150);
}

// Back up for `ms`, then turn away by a random angle. `away` is 1 to turn
// left (the obstacle was on the right) and -1 to turn right.
void retreat(const char *why, unsigned long ms, int away) {
  Serial.println(why);
  drive(-CRUISE, -CRUISE);
  delay(ms);
  int degrees = random(60, 151);
  Serial.print("turning ");
  Serial.print(away > 0 ? "left " : "right ");
  Serial.print(degrees);
  Serial.println(" degrees");
  turn(away * degrees);
  mode = CRUISING;
  modeSince = millis();
}

void setup() {
  pinMode(BUMP_LEFT, INPUT_PULLUP);
  pinMode(BUMP_RIGHT, INPUT_PULLUP);
  pinMode(CLIFF, INPUT);
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  randomSeed(analogRead(A0));
  Serial.begin(115200);
  Serial.println("Mokxi Arduino Uno: robot vacuum behaviors");
  Serial.println("ready: spiraling out");
  modeSince = millis();
}

void loop() {
  // Layer 1: the cliff beats everything.
  if (digitalRead(CLIFF) == HIGH) {
    retreat("cliff: backing up", 450, random(0, 2) ? 1 : -1);
    return;
  }

  // Layer 2: bumpers.
  bool left = digitalRead(BUMP_LEFT) == LOW;
  bool right = digitalRead(BUMP_RIGHT) == LOW;
  if (left || right) {
    retreat(left ? "bumped on the left: backing up" : "bumped on the right: backing up", 350, left ? -1 : 1);
    return;
  }

  // Layer 3: cover the floor.
  unsigned long elapsed = millis() - modeSince;
  if (mode == SPIRAL) {
    if (elapsed > SPIRAL_MS) {
      mode = CRUISING;
      modeSince = millis();
      Serial.println("spiral done: cruising");
      return;
    }
    // The inside wheel starts slow and speeds up, so the circle widens.
    int inside = 70 + elapsed / 45;
    drive(inside, CRUISE);
  } else {
    drive(CRUISE, CRUISE);
  }
  delay(10);
}