Fire fighting robot on an Arduino Uno

// Fire fighting robot: three flame sensors find the fire, a 2WD car on an
// L298N drives to it, and a water pump on a relay puts it out while a servo
// sweeps the nozzle from side to side.
//
//   flame sensors   left DO on A1, center DO on A2, right DO on A3
//                   center AO on A0 (how close the flame is)
//                   VCC 5V, GND; DO goes LOW when a flame is in range
//   L298N           ENA pin 5, IN1 pin 6, IN2 pin 7    left motor
//                   ENB pin 3, IN3 pin 8, IN4 pin 9    right motor
//                   (both EN jumpers off), 12V from the 7.4 V pack
//   nozzle servo    signal on pin 11, 5V and GND
//   pump relay      IN on pin 12; the 6 V pump runs through COM and NO
//
// ENB is on pin 3, not pin 10, because the Servo library takes Timer1 on an
// Uno, and Timer1 is the PWM on pins 9 and 10. With ENB on pin 10 the right
// motor would lose its speed control the moment the servo attaches.
//
// HOW IT FINDS THE FIRE. Each sensor's comparator pulls DO low when the
// infrared it sees passes the trimmer, about half a meter from a lighter at
// the middle setting. The robot:
//
//   1. spins slowly on the spot while no sensor sees a flame (SEARCH),
//   2. turns toward the side sensor that sees one (TURN),
//   3. drives straight at it once the center sensor sees it (APPROACH),
//   4. stops when the center AO says the flame is close, below CLOSE, and
//      runs the pump with the nozzle sweeping (SPRAY),
//   5. keeps spraying until no sensor has seen a flame for OUT_MS, then
//      parks the nozzle and goes back to searching.
//
// AO falls as the flame comes closer (more infrared, more current through
// the phototransistor), so "close" is a low reading. CLOSE = 300 is about
// 40 cm here; raise it to stop farther away, lower it to get closer.
//
// In Mokxi the car stays in the middle of its window, so the distance to the
// fire is the slider on each flame sensor: drag the center one in to 45 cm
// and the car drives, in to 30 cm and it sprays, out to "no flame" and it
// goes back to searching.

#include <Servo.h>

const int ENA = 5, IN1 = 6, IN2 = 7;   // left motor
const int ENB = 3, IN3 = 8, IN4 = 9;   // right motor
const int FLAME_LEFT = A1, FLAME_CENTER = A2, FLAME_RIGHT = A3;
const int FLAME_AO = A0;
const int NOZZLE = 11;
const int PUMP = 12;

const int SEARCH_SPEED = 120;     // slow spin while looking
const int TURN_SPEED = 150;
const int DRIVE_SPEED = 170;
const int CLOSE = 300;            // center AO below this: close enough to spray
const unsigned long OUT_MS = 1000;  // no flame this long: the fire is out

enum State { SEARCH, TURN_LEFT, TURN_RIGHT, APPROACH, SPRAY };
State state = SEARCH;

Servo nozzle;
int nozzleAngle = 90;
int nozzleStep = 3;
unsigned long lastFlame = 0;

// One side of the car, as a signed speed from -255 to 255. Zero brakes.
void side(int en, int a, int b, int s) {
  digitalWrite(a, s > 0 ? HIGH : LOW);
  digitalWrite(b, s < 0 ? HIGH : LOW);
  if (s < 0) s = -s;
  analogWrite(en, s == 0 ? 255 : (s > 255 ? 255 : s));
}

void drive(int left, int right) {
  side(ENA, IN1, IN2, left);
  side(ENB, IN3, IN4, right);
}

bool sees(int pin) {
  return digitalRead(pin) == LOW;
}

void enter(State next, const char *why) {
  if (next == state) return;
  state = next;
  Serial.println(why);
  if (next != SPRAY) {
    digitalWrite(PUMP, LOW);
    nozzleAngle = 90;
    nozzle.write(nozzleAngle);
  }
}

void setup() {
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(PUMP, OUTPUT);
  digitalWrite(PUMP, LOW);
  pinMode(FLAME_LEFT, INPUT);
  pinMode(FLAME_CENTER, INPUT);
  pinMode(FLAME_RIGHT, INPUT);
  nozzle.attach(NOZZLE);
  nozzle.write(nozzleAngle);
  drive(0, 0);
  Serial.begin(9600);
  Serial.println("fire robot: searching");
}

void loop() {
  bool left = sees(FLAME_LEFT);
  bool center = sees(FLAME_CENTER);
  bool right = sees(FLAME_RIGHT);
  int near = analogRead(FLAME_AO);
  if (left || center || right) lastFlame = millis();

  if (state == SPRAY) {
    // Keep spraying until no sensor has seen the flame for OUT_MS.
    if (millis() - lastFlame >= OUT_MS) {
      enter(SEARCH, "fire out: searching");
    } else {
      nozzleAngle += nozzleStep;
      if (nozzleAngle >= 120 || nozzleAngle <= 60) nozzleStep = -nozzleStep;
      nozzle.write(nozzleAngle);
    }
  } else if (center && near < CLOSE) {
    drive(0, 0);
    enter(SPRAY, "close: spraying");
    digitalWrite(PUMP, HIGH);
  } else if (center) {
    enter(APPROACH, "flame ahead: driving to it");
  } else if (left) {
    enter(TURN_LEFT, "flame left: turning left");
  } else if (right) {
    enter(TURN_RIGHT, "flame right: turning right");
  } else {
    enter(SEARCH, "no flame: searching");
  }

  switch (state) {
    case SEARCH: drive(SEARCH_SPEED, -SEARCH_SPEED); break;
    case TURN_LEFT: drive(-TURN_SPEED, TURN_SPEED); break;
    case TURN_RIGHT: drive(TURN_SPEED, -TURN_SPEED); break;
    case APPROACH: drive(DRIVE_SPEED, DRIVE_SPEED); break;
    case SPRAY: drive(0, 0); break;
  }
  delay(20);
}