Factory 6: Splitter and merger, on an Arduino Uno

// Factory 6: Splitter and merger, with jam detection.
//
// One input belt feeds a splitter. The splitter hands items to two lanes in
// turn. A merger pulls from the two lanes in turn and puts everything on a
// single exit belt. A servo on pin 5 is the splitter's gate: it swings to 45
// degrees for the upper lane and 135 for the lower one.
//
// Two knobs set the speeds: A0 is how fast the exit belt takes items away and
// A1 is how fast new items arrive. When the exit is slower than the arrivals
// the lanes fill, then the input queue fills, and items start being turned away
// at the door. When four have been turned away within four seconds the line is
// JAMMED: the alarm sounds, the source stops and the LED blinks until you press
// the button to clear the line.
//
// 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
//   Exit knob (10k pot) -> A0     Arrival knob (10k pot) -> A1
//   Clear button: pin 2 to GND    Gate servo: signal -> pin 5, 5 V, GND
//   Jam LED: pin 3 through 220 ohm to GND      Piezo: pin 6 to GND
//
// # What this teaches
//
// 1. Queues. Each stretch of belt is a queue with a capacity. Items go in at
//    one end and come out of the other in order. Here the queues are just
//    counts, because every item is the same.
// 2. Backpressure. A full queue cannot accept anything, so the stage before it
//    has to wait, and its queue fills, and so on back up the line. A jam is not
//    a sudden break, it is a slow wave moving upstream.
// 3. Fairness. The splitter and the merger both alternate between the two
//    lanes (round robin) so neither lane starves or hogs the exit.
// 4. A threshold over a window. The line is only called jammed when several
//    items are refused within a few seconds, so one busy moment does not set
//    off the alarm but a line that cannot keep up always does.

#include <Servo.h>
#include "mokxi_ili9341.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 BUTTON_PIN = 2;
const uint8_t LED_PIN = 3;
const uint8_t SERVO_PIN = 5;
const uint8_t PIEZO_PIN = 6;
const uint8_t EXIT_KNOB = A0;
const uint8_t ARRIVAL_KNOB = A1;

SoftSpi spi(TFT_SCK, TFT_MOSI, TFT_CS);
Ili9341 tft(spi, TFT_DC);
Servo gate;

const int16_t TILE = 16;
const int16_t WORLD_X = 8;
const int16_t WORLD_Y = 8;
const uint16_t STEEL = 0x8c71;
const uint16_t DARK = 0x10a2;
const uint16_t ORE = 0xfc60;
const uint16_t UPPER = 0x07ff;
const uint16_t LOWER = 0xf81f;
const uint16_t BODY = 0x4a69;

const uint8_t CAPACITY = 4;              // every queue holds four
const unsigned long HOP_MS = 350;        // the splitter moves one item this often
const unsigned long WINDOW_MS = 4000;    // count refused arrivals over this long
const uint8_t JAM_REFUSALS = 4;          // this many in one window is a jam
const uint8_t GATE_UPPER = 45;
const uint8_t GATE_LOWER = 135;
const uint8_t GATE_REST = 90;

// 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 clearButton = {BUTTON_PIN, false, false, 0};

// The three queues, as counts.
uint8_t qIn = 0, qUpper = 0, qLower = 0;
uint8_t shownIn = 255, shownUpper = 255, shownLower = 255;
bool nextUpper = true;        // where the splitter will send the next item
bool takeUpper = true;        // which lane the merger will take from next
unsigned int delivered = 0, routedUpper = 0, routedLower = 0;
unsigned int shownDelivered = 65535;
bool jammed = false;
unsigned long windowStart = 0;  // when the current counting window began
uint8_t refused = 0;            // arrivals turned away in this window
unsigned long lastArrival = 0, lastHop = 0, lastExit = 0, lastBlink = 0;
unsigned long arrivalMs = 1000, exitMs = 800;
long shownArrivalRate = -1, shownExitRate = -1;
int8_t shownStatus = -1;
bool ledOn = false;
bool gateDirty = true;

static int16_t tileX(uint8_t c) { return (int16_t)(WORLD_X + c * TILE); }
static int16_t tileY(uint8_t r) { return (int16_t)(WORLD_Y + r * TILE); }

static void slot(uint8_t col, uint8_t row, uint16_t colour, bool full) {
  tft.fillRect(tileX(col), tileY(row), TILE, TILE, STEEL);
  tft.fillRect((int16_t)(tileX(col) + 3), (int16_t)(tileY(row) + 3), TILE - 6, TILE - 6, full ? colour : DARK);
}

// The input queue sits in columns 1 to 4 with its front next to the splitter, so
// slot 0 is column 4. The lanes run from the splitter at column 5 to the merger
// at column 10, so slot 0 is the one nearest the merger.
static void drawIn(uint8_t from, uint8_t to) {
  for (uint8_t i = from; i < to && i < CAPACITY; i++) {
    slot((uint8_t)(4 - i), 4, ORE, i < qIn);
  }
}
static void drawLane(uint8_t row, uint16_t colour, uint8_t count, uint8_t from, uint8_t to) {
  for (uint8_t i = from; i < to && i < CAPACITY; i++) {
    slot((uint8_t)(9 - i), row, colour, i < count);
  }
}

static void drawMachines() {
  // splitter and merger, with the lane arms joining them
  tft.fillRect(tileX(5), tileY(4), TILE, TILE, BODY);
  tft.fillRect(tileX(10), tileY(4), TILE, TILE, BODY);
  tft.fillRect((int16_t)(tileX(5) + 6), tileY(2) + 6, 4, 2 * TILE + 4, STEEL);
  tft.fillRect((int16_t)(tileX(10) + 6), tileY(2) + 6, 4, 2 * TILE + 4, STEEL);
  // the exit belt
  tft.fillRect(tileX(11), (int16_t)(tileY(4) + 5), 3 * TILE, 6, STEEL);
}

static void drawLabels() {
  tft.text(8, 176, "OUT", ILI9341_WHITE, ILI9341_BLACK, 2);
  tft.text(104, 176, "IN/M", ILI9341_WHITE, ILI9341_BLACK, 2);
  tft.text(8, 198, "EXIT/M", ILI9341_WHITE, ILI9341_BLACK, 2);
}

static int8_t status() {
  if (jammed) return 2;
  if (qIn >= CAPACITY - 1) return 1;
  return 0;
}

static void report() {
  Serial.print("out ");
  Serial.print(delivered);
  Serial.print(" up ");
  Serial.print(routedUpper);
  Serial.print(" low ");
  Serial.print(routedLower);
  Serial.print(" in ");
  Serial.print(qIn);
  Serial.print(" jam ");
  Serial.println(jammed ? 1 : 0);
}

// One drawing job per call.
static void refresh() {
  if (qIn != shownIn) {
    uint8_t old = shownIn == 255 ? 0 : shownIn;
    shownIn = qIn;
    drawIn(old < qIn ? old : qIn, old < qIn ? qIn : old);
  } else if (qUpper != shownUpper) {
    uint8_t old = shownUpper == 255 ? 0 : shownUpper;
    shownUpper = qUpper;
    drawLane(2, UPPER, qUpper, old < qUpper ? old : qUpper, old < qUpper ? qUpper : old);
  } else if (qLower != shownLower) {
    uint8_t old = shownLower == 255 ? 0 : shownLower;
    shownLower = qLower;
    drawLane(6, LOWER, qLower, old < qLower ? old : qLower, old < qLower ? qLower : old);
  } else if (delivered != shownDelivered) {
    shownDelivered = delivered;
    tft.number(8 + 4 * 12, 176, delivered, ILI9341_YELLOW, ILI9341_BLACK, 2, 4);
  } else if (60000L / (long)arrivalMs != shownArrivalRate) {
    shownArrivalRate = 60000L / (long)arrivalMs;
    tft.number(104 + 5 * 12, 176, shownArrivalRate, ILI9341_CYAN, ILI9341_BLACK, 2, 3);
  } else if (60000L / (long)exitMs != shownExitRate) {
    shownExitRate = 60000L / (long)exitMs;
    tft.number(8 + 7 * 12, 198, shownExitRate, ILI9341_CYAN, ILI9341_BLACK, 2, 3);
  } else if (status() != shownStatus) {
    shownStatus = status();
    const char *words = shownStatus == 2 ? "JAM! PRESS" : (shownStatus == 1 ? "BACKING UP" : "FLOWING   ");
    tft.text(8, 220, words, shownStatus == 2 ? ILI9341_RED : (shownStatus == 1 ? ILI9341_ORANGE : ILI9341_GREEN),
             ILI9341_BLACK, 2);
  }
}

static void clearLine() {
  qIn = qUpper = qLower = 0;
  jammed = false;
  refused = 0;
  windowStart = millis();
  digitalWrite(LED_PIN, LOW);
  gate.write(GATE_REST);
  report();
}

void setup() {
  Serial.begin(9600);
  pinMode(BUTTON_PIN, INPUT_PULLUP);
  pinMode(LED_PIN, OUTPUT);
  gate.attach(SERVO_PIN);
  gate.write(GATE_REST);
  tft.begin();
  tft.rect(WORLD_X - 3, WORLD_Y - 3, 14 * TILE + 6, 10 * TILE + 6, 0x4a49, 2);
  for (uint8_t i = 0; i < CAPACITY; i++) {
    slot((uint8_t)(4 - i), 4, ORE, false);
    slot((uint8_t)(9 - i), 2, UPPER, false);
    slot((uint8_t)(9 - i), 6, LOWER, false);
  }
  drawMachines();
  drawLabels();
  for (uint8_t i = 0; i < 12; i++) {
    refresh();
  }
  report();
  lastArrival = lastHop = lastExit = millis();
}

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

  // Knobs: the exit takes an item every 250..1500 ms, arrivals come every 300..2000.
  exitMs = 1500UL - (unsigned long)analogRead(EXIT_KNOB) * 1250UL / 1023UL;
  arrivalMs = 2000UL - (unsigned long)analogRead(ARRIVAL_KNOB) * 1700UL / 1023UL;

  if (pressedOnce(clearButton)) {
    clearLine();
  }

  // Arrivals: a new item joins the input queue unless the line is jammed. If the
  // queue is full the item is turned away, and enough of those in a short
  // while means the line cannot keep up: that is the jam.
  if (!jammed && now - lastArrival >= arrivalMs) {
    lastArrival = now;
    if (qIn < CAPACITY) {
      qIn++;
    } else {
      if (now - windowStart > WINDOW_MS) {
        windowStart = now;
        refused = 0;
      }
      refused++;
      if (refused >= JAM_REFUSALS) {
        jammed = true;
        gate.write(GATE_REST);
        report();
      }
    }
  }

  if (!jammed) {
    // The splitter: send the next item down the lane it is pointing at; if that
    // lane is full, use the other one; if both are full, wait.
    if (qIn > 0 && now - lastHop >= HOP_MS) {
      bool upper = nextUpper;
      if (upper && qUpper >= CAPACITY) {
        upper = false;
      } else if (!upper && qLower >= CAPACITY) {
        upper = true;
      }
      uint8_t room = upper ? qUpper : qLower;
      if (room < CAPACITY) {
        lastHop = now;
        qIn--;
        if (upper) {
          qUpper++;
          routedUpper++;
        } else {
          qLower++;
          routedLower++;
        }
        gate.write(upper ? GATE_UPPER : GATE_LOWER);
        nextUpper = !upper;
      }
    }

    // The merger: take from the lane it is pointing at, or the other if that
    // one is empty, and put the item on the exit belt.
    if (now - lastExit >= exitMs) {
      bool upper = takeUpper;
      if (upper && qUpper == 0) {
        upper = false;
      } else if (!upper && qLower == 0) {
        upper = true;
      }
      uint8_t have = upper ? qUpper : qLower;
      if (have > 0) {
        lastExit = now;
        if (upper) {
          qUpper--;
        } else {
          qLower--;
        }
        takeUpper = !upper;
        delivered++;
        tone(PIEZO_PIN, 900, 20);
        report();
      }
    }
  } else {
    // Jammed: blink the LED and sound the alarm until someone presses the button.
    if (now - lastBlink >= 300) {
      lastBlink = now;
      ledOn = !ledOn;
      digitalWrite(LED_PIN, ledOn ? HIGH : LOW);
      if (ledOn) {
        tone(PIEZO_PIN, 300, 120);
      }
    }
  }

  refresh();
}