Factory 8: Logistics robot, on an Arduino Uno

// Factory 8: Logistics robot. A* finds the way around the machines.
//
// A delivery robot lives at the depot on the left. Crates appear around the
// factory. Steer the yellow cursor with the stick and press the stick to send
// the robot to the crate under it: the robot works out the shortest route,
// walks it a tile at a time, picks the crate up with its gripper, plans a
// route back and delivers it. Press the WALL button to put a wall on the
// cursor's tile or take one away. If a wall lands on the robot's route, it
// plans a new one on the spot.
//
// Wiring, board to parts:
//   TFT   CS -> pin 10   DC -> pin 9   SCK -> pin 13   MOSI -> pin 11
//         RST and LED -> 5 V, VCC -> 5 V, GND -> GND
//   Thumb stick: VRX -> A0, VRY -> A1, SW -> pin 2, VCC -> 5 V, GND -> GND
//   WALL button: pin 4 to GND     Gripper servo: signal -> pin 5, 5 V, GND
//   Busy LED: pin 3 through 220 ohm to GND     Piezo: pin 6 to GND
//
// # What this teaches
//
// 1. A grid is a graph. Every floor tile is a node and every neighbor you can
//    step to is an edge. Walls are just missing edges. That makes "find a way
//    from here to there" a graph search.
// 2. A* search. It keeps a list of tiles still to try (the open set) and always
//    expands the one with the lowest guess of total trip cost f = g + h, where
//    g is the steps taken to reach it and h is the straight-line (Manhattan)
//    distance still to go. The guess never overestimates, so the first time it
//    reaches the goal the route is the shortest. NODES on the screen is how
//    many tiles it had to expand: a good guess searches far less than the
//    whole floor.
// 3. Re-planning. The world can change under a plan. When a wall appears on the
//    remaining route the robot throws the plan away and searches again from
//    where it stands, instead of walking into the wall.
// 4. Memory on a small board. The whole search uses four arrays of 140 bytes,
//    one per tile. Everything is a number from 0 to 139, so no pointers and
//    no dynamic allocation are needed.

#include <Servo.h>
#include "mokxi_ili9341.h"
#include "mokxi_joystick.h"

const uint8_t TFT_CS = 10;
const uint8_t TFT_DC = 9;
const uint8_t TFT_SCK = 13;
const uint8_t TFT_MOSI = 11;
const uint8_t STICK_BUTTON = 2;
const uint8_t LED_PIN = 3;
const uint8_t WALL_PIN = 4;
const uint8_t SERVO_PIN = 5;
const uint8_t PIEZO_PIN = 6;

SoftSpi spi(TFT_SCK, TFT_MOSI, TFT_CS);
Ili9341 tft(spi, TFT_DC);
Joystick stick(A0, A1, STICK_BUTTON);
Servo gripper;

const int16_t TILE = 16;
const int16_t WORLD_X = 8;
const int16_t WORLD_Y = 8;
const uint8_t COLS = 14;
const uint8_t ROWS = 10;
const uint8_t CELLS = COLS * ROWS;

const uint16_t STEEL = 0x8c71;
const uint16_t DARK = 0x10a2;
const uint16_t CRATE = 0xfc60;
const uint16_t ROBOT = ILI9341_CYAN;
const uint16_t CRUMB = 0x07e0;

enum Cell { FLOOR, WALL, BOX };

uint8_t cell[CELLS];
bool crumb[CELLS];            // tiles on the planned route, drawn as dots
uint8_t path[CELLS];          // the planned route, first step first
uint8_t pathLength = 0;
uint8_t pathAt = 0;           // how far along the route the robot is
// A* works in these four arrays, one entry per tile.
uint8_t gScore[CELLS];        // steps from the start, 255 if not reached yet
uint8_t cameFrom[CELLS];      // the tile we arrived from
uint8_t status[CELLS];        // 0 not seen, 1 in the open set, 2 closed
unsigned int nodesExpanded = 0;

const uint8_t DEPOT = 4 * COLS + 0;      // column 0, row 4
uint8_t robotAt = DEPOT;
uint8_t cursorAt = 2 * COLS + 3;         // starts on the first crate
uint8_t goal = 0;
bool carrying = false;
unsigned int delivered = 0;
unsigned int shownDelivered = 65535, shownPath = 65535, shownNodes = 65535;
int8_t shownState = -1;

enum Robot { IDLE, FETCHING, RETURNING, NO_PATH };
Robot mode = IDLE;

unsigned long lastStep = 0, lastCursor = 0, respawnAt = 0;
uint8_t respawnCell = 0;

// A button that reports a press once, after the contacts have stopped bouncing.
struct Button {
  uint8_t pin;
  bool raw;
  bool stable;
  unsigned long since;
};

static bool pressedOnce(Button &b) {
  bool down = digitalRead(b.pin) == LOW;
  unsigned long now = millis();
  if (down != b.raw) {
    b.raw = down;
    b.since = now;
  }
  if (now - b.since >= 20 && b.stable != b.raw) {
    b.stable = b.raw;
    return b.stable;
  }
  return false;
}

Button sendButton = {STICK_BUTTON, false, false, 0};
Button wallButton = {WALL_PIN, false, false, 0};

static uint8_t colOf(uint8_t i) { return (uint8_t)(i % COLS); }
static uint8_t rowOf(uint8_t i) { return (uint8_t)(i / COLS); }
static uint8_t at(uint8_t c, uint8_t r) { return (uint8_t)(r * COLS + c); }
static int16_t px(uint8_t i) { return (int16_t)(WORLD_X + colOf(i) * TILE); }
static int16_t py(uint8_t i) { return (int16_t)(WORLD_Y + rowOf(i) * TILE); }

// ---- drawing ---------------------------------------------------------------

static void drawTile(uint8_t i) {
  int16_t x = px(i), y = py(i);
  tft.fillRect(x, y, TILE, TILE, ILI9341_BLACK);
  if (cell[i] == WALL) {
    tft.fillRect(x, y, TILE, TILE, STEEL);
    tft.fillRect((int16_t)(x + 3), (int16_t)(y + 3), TILE - 6, TILE - 6, DARK);
  } else if (cell[i] == BOX) {
    tft.fillRect((int16_t)(x + 2), (int16_t)(y + 2), TILE - 4, TILE - 4, CRATE);
    tft.fillRect((int16_t)(x + 2), (int16_t)(y + 7), TILE - 4, 2, DARK);
  } else if (i == DEPOT) {
    tft.rect(x, y, TILE, TILE, ILI9341_GREEN, 2);
  } else if (crumb[i]) {
    tft.fillRect((int16_t)(x + 6), (int16_t)(y + 6), 4, 4, CRUMB);
  }
  if (i == robotAt) {
    tft.fillRect((int16_t)(x + 2), (int16_t)(y + 2), TILE - 4, TILE - 4, ROBOT);
    tft.fillRect((int16_t)(x + 6), (int16_t)(y + 6), 4, 4, ILI9341_WHITE);
    if (carrying) {
      tft.fillRect((int16_t)(x + 5), y, 6, 4, CRATE);
    }
  }
  if (i == cursorAt) {
    tft.rect(x, y, TILE, TILE, ILI9341_YELLOW, 2);
  }
}

static void drawLabels() {
  tft.text(8, 176, "DONE", ILI9341_WHITE, ILI9341_BLACK, 2);
  tft.text(104, 176, "PATH", ILI9341_WHITE, ILI9341_BLACK, 2);
  tft.text(8, 198, "NODES", ILI9341_WHITE, ILI9341_BLACK, 2);
}

static const char *modeName() {
  return mode == IDLE ? "READY     " : (mode == FETCHING ? "FETCHING  " : (mode == RETURNING ? "DELIVERING" : "NO PATH   "));
}

static void report() {
  Serial.print("done ");
  Serial.print(delivered);
  Serial.print(" path ");
  Serial.print(pathLength);
  Serial.print(" nodes ");
  Serial.print(nodesExpanded);
  Serial.print(" ");
  switch (mode) {
    case IDLE: Serial.println("READY"); break;
    case FETCHING: Serial.println("FETCHING"); break;
    case RETURNING: Serial.println("DELIVERING"); break;
    default: Serial.println("NOPATH"); break;
  }
}

// One drawing job per call, so the stick and buttons are looked at often.
static void refresh() {
  if (delivered != shownDelivered) {
    shownDelivered = delivered;
    tft.number(8 + 5 * 12, 176, delivered, ILI9341_YELLOW, ILI9341_BLACK, 2, 3);
  } else if (pathLength != shownPath) {
    shownPath = pathLength;
    tft.number(104 + 5 * 12, 176, pathLength, ILI9341_CYAN, ILI9341_BLACK, 2, 3);
  } else if (nodesExpanded != shownNodes) {
    shownNodes = nodesExpanded;
    tft.number(8 + 6 * 12, 198, nodesExpanded, ILI9341_CYAN, ILI9341_BLACK, 2, 3);
  } else if ((int8_t)mode != shownState) {
    shownState = (int8_t)mode;
    tft.text(8, 220, modeName(), mode == NO_PATH ? ILI9341_RED : (mode == IDLE ? ILI9341_GREEN : ILI9341_YELLOW),
             ILI9341_BLACK, 2);
  }
}

// ---- A* --------------------------------------------------------------------

// The guess: how many steps it would take with no walls in the way.
static uint8_t manhattan(uint8_t a, uint8_t b) {
  int dc = (int)colOf(a) - (int)colOf(b);
  int dr = (int)rowOf(a) - (int)rowOf(b);
  return (uint8_t)((dc < 0 ? -dc : dc) + (dr < 0 ? -dr : dr));
}

// Search from `from` to `to`. Returns the number of steps, or -1 if walls make
// it impossible. The route is left in path[].
static int plan(uint8_t from, uint8_t to) {
  for (uint8_t i = 0; i < CELLS; i++) {
    gScore[i] = 255;
    status[i] = 0;
  }
  nodesExpanded = 0;
  gScore[from] = 0;
  status[from] = 1;
  while (true) {
    // Pick the open tile with the lowest f = g + h.
    int best = -1;
    unsigned int bestF = 9999;
    for (uint8_t i = 0; i < CELLS; i++) {
      if (status[i] != 1) {
        continue;
      }
      unsigned int f = (unsigned int)gScore[i] + manhattan(i, to);
      if (f < bestF) {
        bestF = f;
        best = i;
      }
    }
    if (best < 0) {
      return -1;                       // nothing left to try: there is no way
    }
    if (best == to) {
      break;
    }
    status[best] = 2;
    nodesExpanded++;
    uint8_t c = colOf((uint8_t)best), r = rowOf((uint8_t)best);
    for (uint8_t d = 0; d < 4; d++) {
      int nc = c, nr = r;
      if (d == 0) nc = c + 1;
      if (d == 1) nc = c - 1;
      if (d == 2) nr = r + 1;
      if (d == 3) nr = r - 1;
      if (nc < 0 || nc >= COLS || nr < 0 || nr >= ROWS) {
        continue;
      }
      uint8_t n = at((uint8_t)nc, (uint8_t)nr);
      if (cell[n] == WALL || status[n] == 2) {
        continue;
      }
      uint8_t ng = (uint8_t)(gScore[best] + 1);
      if (status[n] == 0 || ng < gScore[n]) {
        gScore[n] = ng;
        cameFrom[n] = (uint8_t)best;
        status[n] = 1;
      }
    }
  }
  // Walk back from the goal to the start to read the route off.
  uint8_t length = gScore[to];
  uint8_t node = to;
  for (int i = length - 1; i >= 0; i--) {
    path[i] = node;
    node = cameFrom[node];
  }
  return length;
}

// Plan a route to `to`, draw its dots, and report whether there is one.
static bool startRoute(uint8_t to) {
  // forget the old dots
  for (uint8_t i = 0; i < pathLength; i++) {
    uint8_t t = path[i];
    if (crumb[t]) {
      crumb[t] = false;
      if (i >= pathAt) {
        drawTile(t);
      }
    }
  }
  int length = plan(robotAt, to);
  if (length < 0) {
    pathLength = 0;
    pathAt = 0;
    return false;
  }
  pathLength = (uint8_t)length;
  pathAt = 0;
  goal = to;
  for (uint8_t i = 0; i + 1 < pathLength; i++) {
    crumb[path[i]] = true;
    drawTile(path[i]);
  }
  return true;
}

// ---- the robot ---------------------------------------------------------------

static void setMode(Robot next) {
  mode = next;
  digitalWrite(LED_PIN, next == FETCHING || next == RETURNING ? HIGH : LOW);
  report();
}

static void moveRobotTo(uint8_t tile) {
  uint8_t old = robotAt;
  robotAt = tile;
  drawTile(old);
  drawTile(tile);
}

static void arrive() {
  if (mode == FETCHING) {
    cell[robotAt] = FLOOR;
    carrying = true;
    respawnCell = robotAt;
    respawnAt = millis() + 2500;
    gripper.write(20);                 // close the gripper on the crate
    tone(PIEZO_PIN, 900, 60);
    moveRobotTo(robotAt);
    if (startRoute(DEPOT)) {
      setMode(RETURNING);
    } else {
      setMode(NO_PATH);
    }
  } else if (mode == RETURNING) {
    carrying = false;
    delivered++;
    gripper.write(100);                // open it over the depot
    tone(PIEZO_PIN, 1200, 80);
    moveRobotTo(robotAt);
    pathLength = 0;
    setMode(IDLE);
  }
}

// A wall has just changed: if it is on the rest of the route, plan again.
static void replanIfBlocked() {
  if (mode != FETCHING && mode != RETURNING) {
    return;
  }
  for (uint8_t i = pathAt; i < pathLength; i++) {
    if (cell[path[i]] == WALL) {
      if (!startRoute(goal)) {
        setMode(NO_PATH);
      } else {
        report();
      }
      return;
    }
  }
}

void setup() {
  Serial.begin(9600);
  pinMode(WALL_PIN, INPUT_PULLUP);
  pinMode(LED_PIN, OUTPUT);
  stick.begin();
  gripper.attach(SERVO_PIN);
  gripper.write(100);
  // The machines: a long wall with gaps at the top and bottom, and two blocks.
  for (uint8_t r = 1; r <= 7; r++) {
    cell[at(6, r)] = WALL;
  }
  for (uint8_t r = 3; r <= 5; r++) {
    cell[at(9, r)] = WALL;
  }
  cell[at(3, 7)] = WALL;
  cell[at(4, 7)] = WALL;
  // Three crates.
  cell[at(3, 2)] = BOX;
  cell[at(11, 2)] = BOX;
  cell[at(11, 7)] = BOX;
  tft.begin();
  tft.rect(WORLD_X - 3, WORLD_Y - 3, 14 * TILE + 6, 10 * TILE + 6, 0x4a49, 2);
  for (uint8_t i = 0; i < CELLS; i++) {
    drawTile(i);
  }
  drawLabels();
  for (uint8_t i = 0; i < 8; i++) {
    refresh();
  }
  report();
}

void loop() {
  unsigned long now = millis();

  // The cursor follows the stick, one tile at a time.
  int8_t dx = stick.stepX();
  int8_t dy = stick.stepY();
  if ((dx != 0 || dy != 0) && now - lastCursor >= 150) {
    lastCursor = now;
    int nc = (int)colOf(cursorAt) + dx;
    int nr = (int)rowOf(cursorAt) + dy;
    if (nc >= 0 && nc < COLS && nr >= 0 && nr < ROWS) {
      uint8_t old = cursorAt;
      cursorAt = at((uint8_t)nc, (uint8_t)nr);
      drawTile(old);
      drawTile(cursorAt);
    }
  }

  // Pressing the stick sends the robot to the crate under the cursor.
  if (pressedOnce(sendButton) && (mode == IDLE || mode == NO_PATH)) {
    if (cell[cursorAt] == BOX) {
      if (startRoute(cursorAt)) {
        setMode(FETCHING);
      } else {
        setMode(NO_PATH);
        tone(PIEZO_PIN, 200, 200);
      }
    } else {
      tone(PIEZO_PIN, 200, 80);          // nothing to fetch there
    }
  }

  // The WALL button puts a wall on the cursor's tile, or removes one. Not on the
  // depot, the robot or a crate.
  if (pressedOnce(wallButton)) {
    if (cursorAt != DEPOT && cursorAt != robotAt && cell[cursorAt] != BOX) {
      cell[cursorAt] = cell[cursorAt] == WALL ? FLOOR : WALL;
      drawTile(cursorAt);
      tone(PIEZO_PIN, 600, 30);
      replanIfBlocked();
    }
  }

  // The robot walks its route, a tile every 130 ms.
  if ((mode == FETCHING || mode == RETURNING) && now - lastStep >= 130) {
    lastStep = now;
    if (pathAt < pathLength) {
      uint8_t next = path[pathAt++];
      crumb[next] = false;
      moveRobotTo(next);
      if (pathAt >= pathLength) {
        arrive();
      }
    }
  }

  // A delivered crate is replaced a moment later where it came from.
  if (respawnAt != 0 && now >= respawnAt && cell[respawnCell] == FLOOR && respawnCell != robotAt) {
    respawnAt = 0;
    cell[respawnCell] = BOX;
    drawTile(respawnCell);
  }

  refresh();
}