Pick and place on an Arduino Uno

// Pick and place: a two-axis gantry that homes itself, then moves a part
// from a pick point to a place point each time START is pressed.
//
//   7 6 5 4     X axis, a 28BYJ-48 on its ULN2003 board (IN1 to IN4)
//   11 10 9 8   Y axis, a second 28BYJ-48 (IN1 to IN4)
//   12          relay, the vacuum pump (the board switches on a low input)
//   13          Z axis, a servo that lowers the nozzle (red on 5 V, brown on GND)
//   2           X home switch, COM to GND (pressed = low)
//   3           Y home switch, COM to GND (pressed = low)
//   A0          START button to GND (pressed = low)
//   LCD         SDA -> A4    SCL -> A5    VCC -> 5 V    GND -> GND
//
// A stepper only knows how many steps it has taken, not where it is. So at
// power up the machine homes: each axis creeps back until its switch clicks,
// and that spot becomes step 0. From then on every position is a step count.
//
// One cycle is a list of states, and each one waits for the last to finish:
//
//   TO PICK     X runs to the pick point, then Y does (one axis at a time)
//   PICK DOWN   Z lowers the nozzle onto the part
//   GRIP        vacuum on, only now that the nozzle is down
//   PICK UP     Z lifts the part
//   TO PLACE    X, then Y, to the place point
//   PLACE DOWN  Z lowers the part
//   RELEASE     vacuum off, so the part stays put
//   PLACE UP    Z lifts the empty nozzle
//   RETURN      X, then Y, back to 0; the part is counted
//
// The order is the lesson: vacuum before the nozzle is down grabs air, and
// vacuum off before it is down drops the part. START while a cycle runs is
// ignored. The loop never waits with delay(); it checks the clock, so the
// steppers, the servo and the button are all looked after every pass.
//
// Press Run, click the X and then the Y home switch, then press START.

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

const int X_HOME_PIN = 2;
const int Y_HOME_PIN = 3;
const int VACUUM_PIN = 12;
const int Z_PIN = 13;
const int START_PIN = A0;
const int X_PINS[4] = {7, 6, 5, 4};      // IN1 to IN4
const int Y_PINS[4] = {11, 10, 9, 8};    // IN1 to IN4

// Positions in half-steps: 4096 make one turn of a 28BYJ-48's shaft.
const long PICK_X = 512;
const long PICK_Y = 256;
const long PLACE_X = 1536;
const long PLACE_Y = 768;

const unsigned long STEP_US = 2000;   // one half-step every 2 ms
const unsigned long Z_MS = 500;       // time for the nozzle to go down or up
const unsigned long GRIP_MS = 400;    // vacuum building up
const unsigned long RELEASE_MS = 400; // vacuum dying away
const unsigned long DEBOUNCE_MS = 30;
const int Z_UP = 20;
const int Z_DOWN = 110;

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

// The eight-step half-step pattern, IN1 in bit 0.
const byte HALF_STEPS[8] = {0b0001, 0b0011, 0b0010, 0b0110, 0b0100, 0b1100, 0b1000, 0b1001};

enum State {
  HOME_X, HOME_Y, IDLE, TO_PICK, PICK_DOWN, GRIP, PICK_UP,
  TO_PLACE, PLACE_DOWN, RELEASE, PLACE_UP, RETURN
};
const char* const NAMES[] = {
  "HOME X", "HOME Y", "IDLE", "TO PICK", "PICK DOWN", "GRIP", "PICK UP",
  "TO PLACE", "PLACE DOWN", "RELEASE", "PLACE UP", "RETURN"
};

struct Axis {
  const int* pins;
  long pos;     // half-steps from home
  byte phase;   // where in HALF_STEPS the coils are
};

Axis xAxis = {X_PINS, 0, 0};
Axis yAxis = {Y_PINS, 0, 0};

LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo z(Z_PIN);

State state = HOME_X;
unsigned long enteredAt = 0;
unsigned long lastStep = 0;   // micros() of the last half-step
unsigned long cycles = 0;
int zAngle = Z_UP;
long targetX = 0;
long targetY = 0;
bool startWas = false;
unsigned long startChanged = 0;
bool startStable = false;

void coils(Axis& a, byte pattern) {
  for (int i = 0; i < 4; i++) digitalWrite(a.pins[i], (pattern >> i) & 1);
}

// One half-step forward (+1) or back (-1).
void step(Axis& a, int dir) {
  a.phase = (a.phase + 8 + dir) % 8;
  a.pos += dir;
  coils(a, HALF_STEPS[a.phase]);
}

// Coils off when an axis is not moving: it stays put and does not get hot.
void rest(Axis& a) {
  coils(a, 0);
}

// The LCD is slow (every character is an I2C message), so only what changed
// is written: the state on row 0, the count on row 1 when it moves.
void show() {
  lcd.setCursor(0, 0);
  lcd.print(NAMES[state]);
  for (int i = strlen(NAMES[state]); i < 10; i++) lcd.print(' ');
}

void showCount() {
  lcd.setCursor(7, 1);
  lcd.print(cycles);
}

void enter(State next) {
  state = next;
  enteredAt = millis();
  // The vacuum is decided here, in one place, for every state.
  bool vacuum = (next == GRIP || next == PICK_UP || next == TO_PLACE || next == PLACE_DOWN);
  digitalWrite(VACUUM_PIN, vacuum ? ON : OFF);
  if (next == TO_PICK) { targetX = PICK_X; targetY = PICK_Y; }
  if (next == TO_PLACE) { targetX = PLACE_X; targetY = PLACE_Y; }
  if (next == RETURN) { targetX = 0; targetY = 0; }
  Serial.print("state ");
  Serial.println(NAMES[next]);
  show();
}

// Whether the next half-step is due. Timed in microseconds: with millis()
// two steps "2 ms apart" can land barely 1 ms apart, faster than the rotor
// can follow, and the motor quietly loses steps.
bool stepDue() {
  if (micros() - lastStep < STEP_US) return false;
  lastStep = micros();
  return true;
}

// Move X to its target, then Y. True once both are there.
bool moveTo() {
  if (xAxis.pos == targetX && yAxis.pos == targetY) return true;
  if (!stepDue()) return false;
  if (xAxis.pos != targetX) {
    step(xAxis, xAxis.pos < targetX ? 1 : -1);
    if (xAxis.pos == targetX) rest(xAxis);
    return false;
  }
  step(yAxis, yAxis.pos < targetY ? 1 : -1);
  if (yAxis.pos == targetY) rest(yAxis);
  return false;
}

// Creep an axis back toward its switch. True once the switch is pressed.
bool home(Axis& a, int switchPin) {
  if (digitalRead(switchPin) == LOW) {
    rest(a);
    a.pos = 0;
    return true;
  }
  if (stepDue()) step(a, -1);
  return false;
}

// A clean press of START: low for DEBOUNCE_MS after being high.
bool startPressed(unsigned long now) {
  bool down = digitalRead(START_PIN) == LOW;
  if (down != startWas) {
    startWas = down;
    startChanged = now;
  }
  if (down != startStable && now - startChanged >= DEBOUNCE_MS) {
    startStable = down;
    return down;
  }
  return false;
}

void setup() {
  pinMode(X_HOME_PIN, INPUT_PULLUP);
  pinMode(Y_HOME_PIN, INPUT_PULLUP);
  pinMode(START_PIN, INPUT_PULLUP);
  pinMode(VACUUM_PIN, OUTPUT);
  digitalWrite(VACUUM_PIN, OFF);
  for (int i = 0; i < 4; i++) {
    pinMode(X_PINS[i], OUTPUT);
    pinMode(Y_PINS[i], OUTPUT);
  }
  Serial.begin(9600);
  Serial.println("Pick and place");
  z.begin();
  z.write(Z_UP);
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 1);
  lcd.print("Parts: 0");
  enter(HOME_X);
}

void loop() {
  unsigned long now = millis();
  bool start = startPressed(now);
  int wantZ = Z_UP;

  switch (state) {
    case HOME_X:
      if (home(xAxis, X_HOME_PIN)) enter(HOME_Y);
      break;
    case HOME_Y:
      if (home(yAxis, Y_HOME_PIN)) {
        Serial.println("homed");
        enter(IDLE);
      }
      break;
    case IDLE:
      if (start) enter(TO_PICK);
      start = false;
      break;
    case TO_PICK:
      if (moveTo()) enter(PICK_DOWN);
      break;
    case PICK_DOWN:
      wantZ = Z_DOWN;
      if (now - enteredAt >= Z_MS) enter(GRIP);
      break;
    case GRIP:
      wantZ = Z_DOWN;
      if (now - enteredAt >= GRIP_MS) enter(PICK_UP);
      break;
    case PICK_UP:
      if (now - enteredAt >= Z_MS) enter(TO_PLACE);
      break;
    case TO_PLACE:
      if (moveTo()) enter(PLACE_DOWN);
      break;
    case PLACE_DOWN:
      wantZ = Z_DOWN;
      if (now - enteredAt >= Z_MS) enter(RELEASE);
      break;
    case RELEASE:
      wantZ = Z_DOWN;
      if (now - enteredAt >= RELEASE_MS) enter(PLACE_UP);
      break;
    case PLACE_UP:
      if (now - enteredAt >= Z_MS) enter(RETURN);
      break;
    case RETURN:
      if (moveTo()) {
        cycles++;
        Serial.print("parts ");
        Serial.println(cycles);
        showCount();
        enter(IDLE);
      }
      break;
  }

  // A press that no state used: the machine is busy.
  if (start && state != HOME_X && state != HOME_Y) Serial.println("busy, start ignored");

  if (wantZ != zAngle) {
    zAngle = wantZ;
    z.write(zAngle);
  }
  z.refresh();
}