Sumo robot on an Arduino Uno

// Sumo robot: push the other robot out of the ring and stay in it yourself.
//
//   HC-SR04 on the front: TRIG pin 12, ECHO pin 11
//   Two TCRT5000 line sensors on the front corners: left DO on pin 7, right DO on pin 8
//   DRV8833 driver: AIN1 pin 5, AIN2 pin 6 (left motor)   BIN1 pin 9, BIN2 pin 10 (right motor)
//
// # The ring
//
// The ring is white board with a black tape border. A line sensor's DO pin
// reads LOW over white and HIGH over black, so HIGH on either front corner
// means that corner has reached the edge.
//
// # Four states
//
//   SEARCH   spin on the spot, one way and then the other, and ping
//   CHARGE   the other robot is closer than 40 cm: full speed straight at it
//   BACKOFF  an edge sensor saw the border: reverse so it is not pushed over
//   TURN     swing away from the side that saw the border, then search again
//
// A state machine is a robot that is always in exactly one state and has a
// short list of reasons to change. The border always wins: the check for it
// comes first in loop(), because a robot that charges off the ring has lost no
// matter how good its eyes are.
//
// # The ultrasonic eye
//
// The sensor ignores a trigger that comes less than 60 ms after the last one,
// and a ping sent right on the mark can be ignored too, which reads as "nothing
// there" and makes the robot flicker between charging and searching. So the
// pings are 70 ms apart. pulseIn() gets a timeout so that an empty ring costs
// 8 ms a ping and not the sensor's full 38 ms, which would leave the robot blind
// to the border for a long time.
//
// # Starting
//
// Sumo rules make a robot wait before it moves. This one waits three seconds.

const int TRIG = 12;
const int ECHO = 11;
const int EDGE_LEFT = 7;
const int EDGE_RIGHT = 8;

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

const int CHARGE_CM = 40;
const int PING_GAP_MS = 70;
const unsigned long START_DELAY_MS = 3000;
const unsigned long BACKOFF_MS = 350;
const unsigned long TURN_MS = 320;
const unsigned long SPIN_MS = 1400;

const int FULL = 255;
const int SPIN = 150;

enum State { SEARCH, CHARGE, BACKOFF, TURN };

State state = SEARCH;
unsigned long stateSince = 0;
unsigned long lastPing = 0;
int spinDirection = 1;
int turnDirection = 1;
int distanceCm = 999;

// PWM on one input with the other low is the speed; swap them to reverse.
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);
}

void enter(State next, const char *why) {
  if (state != next) {
    Serial.println(why);
  }
  state = next;
  stateSince = millis();
}

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

void setup() {
  Serial.begin(115200);
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
  pinMode(EDGE_LEFT, INPUT);
  pinMode(EDGE_RIGHT, INPUT);
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  Serial.println("Mokxi Arduino Uno: sumo robot");
  Serial.println("ready: starting in 3 seconds");
  delay(START_DELAY_MS);
  Serial.println("fight");
  enter(SEARCH, "");
}

void loop() {
  unsigned long now = millis();
  bool leftEdge = digitalRead(EDGE_LEFT) == HIGH;
  bool rightEdge = digitalRead(EDGE_RIGHT) == HIGH;

  // The border beats everything else, whatever state the robot is in.
  if ((leftEdge || rightEdge) && state != BACKOFF && state != TURN) {
    // Turn away from the side that saw it; both at once, turn right.
    turnDirection = leftEdge ? 1 : -1;
    enter(BACKOFF, leftEdge ? "edge on the left: backing off" : "edge on the right: backing off");
  }

  if (state == BACKOFF) {
    drive(-FULL, -FULL);
    if (now - stateSince > BACKOFF_MS) enter(TURN, "");
    return;
  }
  if (state == TURN) {
    drive(SPIN * turnDirection, -SPIN * turnDirection);
    if (now - stateSince > TURN_MS) enter(SEARCH, "searching");
    return;
  }

  if (now - lastPing >= PING_GAP_MS) {
    lastPing = now;
    distanceCm = ping();
  }

  if (distanceCm < CHARGE_CM) {
    if (state != CHARGE) {
      Serial.print("opponent at ");
      Serial.print(distanceCm);
      Serial.println(" cm: charging");
    }
    state = CHARGE;
    drive(FULL, FULL);
    return;
  }

  if (state == CHARGE) enter(SEARCH, "lost it: searching");

  // Search: spin one way, then the other, so a robot that started facing
  // away from the other one finds it either side.
  if (now - stateSince > SPIN_MS) {
    spinDirection = -spinDirection;
    stateSince = now;
  }
  drive(SPIN * spinDirection, -SPIN * spinDirection);
}