Reject station on an Arduino Uno

// Reject station: a quality check on a conveyor. Each part is measured as
// it passes the gate, and a pusher further down the belt kicks the bad ones
// into the reject bin when they get there.
//
//   2       E18-D80NK, OUT (brown on 5 V, blue on GND): low while a part
//           stands at the gate
//   7/6     HC-SR04 TRIG and ECHO, looking down at the belt from 100 mm up
//   9       relay board IN1, the conveyor motor's contactor
//   8       relay board IN2, the reject horn (a small siren on the contacts)
//   13      the pusher, a servo (red on 5 V, brown on GND)
//   12/11/10 the stack light: red, yellow, green
//   LCD     SDA -> A4    SCL -> A5    VCC -> 5 V    GND -> GND
//
// The idea is tracking. The gate and the pusher are in different places,
// so the sensor and the actuator act at different times:
//
//   1. A part arrives at the gate. The ultrasonic sensor measures its height
//      and the sketch decides, right then, good or bad.
//   2. The decision goes into a queue with the time the part arrived. The
//      part rides on down the belt.
//   3. TRAVEL_MS later the part is at the pusher. The sketch takes the oldest
//      entry off the queue: a bad part gets kicked, a good part rides on.
//
// Several parts can be on the belt between the gate and the pusher at once,
// and the queue keeps them in order. The loop never waits with delay(), so
// the gate is watched the whole time the pusher is working.
//
// The stack light: green while all is well, yellow while a bad part is on its
// way to the pusher, red while the pusher is out.
//
// Press Run, set the ultrasonic sensor's range (60 mm is a good 40 mm part),
// then drag the E18-D80NK's target close and away again for each part.

#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include "mokxi_servo.h"

const int GATE_PIN = 2;
const int TRIG_PIN = 7;
const int ECHO_PIN = 6;
const int CONVEYOR_PIN = 9;
const int HORN_PIN = 8;
const int PUSHER_PIN = 13;
const int RED_PIN = 12;
const int YELLOW_PIN = 11;
const int GREEN_PIN = 10;

const int MOUNT_MM = 100;              // the sensor's height above the belt
const int TARGET_MM = 40;              // a good part's height
const int TOLERANCE_MM = 3;            // plus or minus
const unsigned long TRAVEL_MS = 2000;  // gate to pusher at belt speed
const unsigned long KICK_MS = 400;     // pusher out
const unsigned long BEEP_MS = 150;
const unsigned long CLEAR_MS = 50;     // the gate must be clear between parts
const int PUSHER_HOME = 0;
const int PUSHER_OUT = 90;

// The relay board switches on a low input.
const int ON = LOW;
const int OFF = HIGH;

// The queue: one entry for every part between the gate and the pusher,
// oldest first, in a ring of QUEUE_SIZE slots.
const int QUEUE_SIZE = 8;
unsigned long arrivedAt[QUEUE_SIZE];
bool isBad[QUEUE_SIZE];
int head = 0;   // the oldest entry
int count = 0;  // entries in the queue
int badOnBelt = 0;  // bad parts among them

LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo pusher(PUSHER_PIN);

unsigned int parts = 0;
unsigned int good = 0;
unsigned int bad = 0;
bool present = false;
unsigned long lastBlocked = 0;
unsigned long kickUntil = 0;
unsigned long beepUntil = 0;
bool kicking = false;

// One reading from the HC-SR04, in millimetres; 0 when there is no echo.
int distanceMm() {
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  unsigned long us = pulseIn(ECHO_PIN, HIGH, 30000UL);
  // Sound covers 0.343 mm a microsecond, there and back.
  return (int)((us * 343UL + 1000UL) / 2000UL);
}

void show() {
  lcd.setCursor(0, 0);
  lcd.print("Good ");
  lcd.print(good);
  lcd.print("  Bad ");
  lcd.print(bad);
  lcd.print("    ");
  lcd.setCursor(0, 1);
  lcd.print("Reject ");
  lcd.print(parts ? (unsigned long)bad * 100UL / parts : 0UL);
  lcd.print("%     ");
}

void light() {
  bool red = kicking;
  bool yellow = !red && badOnBelt > 0;
  digitalWrite(RED_PIN, red);
  digitalWrite(YELLOW_PIN, yellow);
  digitalWrite(GREEN_PIN, !red && !yellow);
}

// Step 1 and 2: measure, decide, and remember the decision.
void inspect(unsigned long now) {
  parts++;
  int distance = distanceMm();
  int height = distance > 0 ? MOUNT_MM - distance : 0;
  bool reject = height < TARGET_MM - TOLERANCE_MM || height > TARGET_MM + TOLERANCE_MM;
  if (reject) bad++; else good++;
  Serial.print("part ");
  Serial.print(parts);
  Serial.print(": ");
  Serial.print(height);
  Serial.println(reject ? " mm, bad" : " mm, good");
  // Eight slots: parts closer than a quarter of a second apart would be
  // more than the belt can carry anyway.
  if (count < QUEUE_SIZE) {
    int tail = (head + count) % QUEUE_SIZE;
    arrivedAt[tail] = now;
    isBad[tail] = reject;
    count++;
    if (reject) badOnBelt++;
  }
  show();
}

// Step 3: the oldest part has reached the pusher.
void atPusher(unsigned long now) {
  unsigned int n = parts - count + 1;  // the queue holds the newest parts in order
  bool reject = isBad[head];
  head = (head + 1) % QUEUE_SIZE;
  count--;
  if (reject) {
    badOnBelt--;
    kicking = true;
    kickUntil = now + KICK_MS;
    digitalWrite(HORN_PIN, ON);
    beepUntil = now + BEEP_MS;
  }
  Serial.print(reject ? "kick" : "pass");
  Serial.print(" part ");
  Serial.println(n);
}

void setup() {
  pinMode(GATE_PIN, INPUT_PULLUP);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  digitalWrite(CONVEYOR_PIN, OFF);
  digitalWrite(HORN_PIN, OFF);
  pinMode(CONVEYOR_PIN, OUTPUT);
  pinMode(HORN_PIN, OUTPUT);
  pinMode(RED_PIN, OUTPUT);
  pinMode(YELLOW_PIN, OUTPUT);
  pinMode(GREEN_PIN, OUTPUT);
  Serial.begin(9600);
  Serial.println("Reject station");
  pusher.begin();
  pusher.write(PUSHER_HOME);
  lcd.init();
  lcd.backlight();
  show();
  digitalWrite(CONVEYOR_PIN, ON);  // ready: the belt runs from here on
  light();
}

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

  // A new part at the gate: the beam was clear a moment ago, long enough.
  bool blocked = digitalRead(GATE_PIN) == LOW;
  if (blocked) {
    if (!present && now - lastBlocked >= CLEAR_MS) inspect(now);
    present = true;
    lastBlocked = now;
  } else {
    present = false;
  }

  // The oldest part on the belt reaches the pusher TRAVEL_MS after the gate.
  if (count > 0 && now - arrivedAt[head] >= TRAVEL_MS) atPusher(now);

  if (kicking && (long)(now - kickUntil) >= 0) kicking = false;
  if (beepUntil != 0 && (long)(now - beepUntil) >= 0) {
    digitalWrite(HORN_PIN, OFF);
    beepUntil = 0;
  }

  light();
  pusher.write(kicking ? PUSHER_OUT : PUSHER_HOME);
  delay(20);
}