ASRS picker on an Arduino Uno

// ASRS picker: a storage carriage that homes itself, counts its steps to a
// shelf, picks with a fork and comes back to the dock.
//
//   7 6 5 4      28BYJ-48 board IN1 to IN4 (+ to 5 V, - to GND)
//   2            home limit microswitch, COM to GND and NO to the pin
//   9            the fork, a servo (red on 5 V, brown on GND)
//   A3 A2 A1 A0  keypad rows R1 to R4
//   13 12 11     keypad columns C1 to C3
//   3            buzzer, + to the pin and - to GND
//   LCD          SDA -> A4    SCL -> A5    VCC -> 5 V    GND -> GND
//
// A stepper has no idea where it is. It only knows how many steps it has been
// told to take. So an automated store starts every day the same way:
//
//   HOMING   creep toward the home switch until it closes.
//   BACKOFF  step off the switch, then call that spot step zero.
//   READY    type a shelf 1 to 6 and press #. Anything else is refused with
//            a beep: a soft limit in code, before the motor ever moves.
//   TRAVEL   count steps out to shelf * SHELF_STEPS.
//   FORK     the fork reaches in, then pulls back with the box.
//   RETURN   count steps back to zero, the dock.
//   FAULT    the home switch closed while the carriage was out on the rail.
//            Something moved it or the count is wrong, so it stops dead and
//            beeps. * homes again.
//
// From then on position is open loop: the count is the position, as long as
// no step is missed. The limit switch is the check on that, a hard safety
// behind the soft limit.
//
// Press Run, and press the switch to tell the carriage it has reached home.

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

const int COIL_PINS[4] = {7, 6, 5, 4};  // IN1 to IN4
const int HOME_PIN = 2;
const int FORK_PIN = 9;
const int BUZZER_PIN = 3;

const int SHELVES = 6;
const long SHELF_STEPS = 512;           // half-steps between shelves, 45 degrees
const long BACKOFF_STEPS = 200;         // off the switch before zeroing
const long HOMING_MAX = SHELF_STEPS * 8; // no switch after this: fault
const unsigned long TRAVEL_US = 2000;   // one half-step every 2 ms
const unsigned long CREEP_US = 4000;    // slower while looking for home
const unsigned long REACH_MS = 800;     // fork out
const unsigned long PULL_MS = 600;      // fork back in
const int FORK_IN = 0;
const int FORK_OUT = 90;

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

const byte ROWS = 4;
const byte COLS = 3;
char keys[ROWS][COLS] = {
  {'1', '2', '3'},
  {'4', '5', '6'},
  {'7', '8', '9'},
  {'*', '0', '#'},
};
byte rowPins[ROWS] = {A3, A2, A1, A0};
byte colPins[COLS] = {13, 12, 11};
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

enum State { HOMING, BACKOFF, READY, TRAVEL, FORK, RETURN, FAULT };
const char* const NAMES[] = {"HOMING", "BACKOFF", "READY", "TRAVEL", "FORK", "RETURN", "FAULT"};

LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo fork(FORK_PIN);

State state = HOMING;
unsigned long enteredAt = 0;
unsigned long steppedAt = 0;
long position = 0;      // half-steps from the dock
long moved = 0;         // half-steps taken in this state
byte phase = 0;         // where the coils are in HALF_STEPS
int typed = 0;          // the shelf typed so far, 0 for none
int target = 0;         // the shelf being fetched
int forkAngle = FORK_IN;
unsigned long beepUntil = 0;

bool atHome() {
  return digitalRead(HOME_PIN) == LOW;
}

// One half-step forward (+1) or back (-1), and the coils follow the count.
void step(int dir) {
  position += dir;
  moved++;
  // The phase follows the coils, not the count, so zeroing the count at home
  // does not make the rotor jump.
  phase = (phase + dir) & 7;
  byte pattern = HALF_STEPS[phase];
  for (int i = 0; i < 4; i++) digitalWrite(COIL_PINS[i], (pattern >> i) & 1);
}

void release() {
  for (int i = 0; i < 4; i++) digitalWrite(COIL_PINS[i], LOW);
}

void beep(unsigned long ms) {
  beepUntil = millis() + ms;
  digitalWrite(BUZZER_PIN, HIGH);
}

void show() {
  lcd.setCursor(0, 0);
  lcd.print(NAMES[state]);
  lcd.print("          ");
  lcd.setCursor(0, 1);
  if (state == READY) {
    lcd.print("Shelf? ");
    if (typed) lcd.print(typed);
  } else {
    lcd.print("Target ");
    lcd.print(target);
  }
  lcd.print("         ");
}

void enter(State next) {
  state = next;
  enteredAt = millis();
  moved = 0;
  if (next == READY || next == FORK || next == FAULT) release();
  Serial.print("state ");
  Serial.println(NAMES[next]);
  show();
}

void setup() {
  for (int i = 0; i < 4; i++) pinMode(COIL_PINS[i], OUTPUT);
  pinMode(HOME_PIN, INPUT_PULLUP);
  pinMode(BUZZER_PIN, OUTPUT);
  Serial.begin(9600);
  Serial.println("ASRS picker");
  fork.begin();
  fork.write(FORK_IN);
  lcd.init();
  lcd.backlight();
  enter(HOMING);
}

void handleKey(char key) {
  if (key == '*' && state == FAULT) {
    enter(HOMING);
    return;
  }
  if (state != READY) return;
  if (key >= '0' && key <= '9') {
    typed = key - '0';
    show();
  } else if (key == '#') {
    // The soft limit: refuse a shelf that is not there before moving.
    if (typed < 1 || typed > SHELVES) {
      Serial.print("refused shelf ");
      Serial.println(typed);
      beep(300);
      typed = 0;
      show();
      return;
    }
    target = typed;
    typed = 0;
    Serial.print("fetch shelf ");
    Serial.println(target);
    enter(TRAVEL);
  }
}

// Steps toward goal at one half-step per period; true once it is there.
bool moveTo(long goal, unsigned long period) {
  if (position == goal) return true;
  unsigned long nowUs = micros();
  if (nowUs - steppedAt >= period) {
    steppedAt = nowUs;
    step(goal > position ? 1 : -1);
  }
  return false;
}

void loop() {
  char key = keypad.getKey();
  if (key) handleKey(key);
  unsigned long now = millis();

  switch (state) {
    case HOMING:
      if (atHome()) {
        Serial.println("home switch");
        enter(BACKOFF);
      } else if (moved >= HOMING_MAX) {
        Serial.println("fault: no home switch");
        beep(1000);
        enter(FAULT);
      } else {
        moveTo(position - 1, CREEP_US);
      }
      break;
    case BACKOFF:
      if (moved >= BACKOFF_STEPS) {
        if (atHome()) {
          Serial.println("fault: switch stuck");
          beep(1000);
          enter(FAULT);
        } else {
          // This spot is the dock: the count starts here.
          position = 0;
          Serial.println("homed, step 0");
          enter(READY);
        }
      } else {
        moveTo(position + 1, CREEP_US);
      }
      break;
    case TRAVEL:
    case RETURN:
      // The hard limit: out on the rail the switch must stay open.
      if (atHome()) {
        Serial.println("fault: limit hit");
        beep(1000);
        enter(FAULT);
      } else if (moveTo(state == TRAVEL ? target * SHELF_STEPS : 0, TRAVEL_US)) {
        if (state == TRAVEL) {
          Serial.print("at shelf ");
          Serial.println(target);
          enter(FORK);
        } else {
          Serial.println("docked");
          target = 0;
          enter(READY);
        }
      }
      break;
    case FORK: {
      int want = now - enteredAt < REACH_MS ? FORK_OUT : FORK_IN;
      if (want != forkAngle) {
        forkAngle = want;
        fork.write(forkAngle);
        Serial.println(forkAngle == FORK_OUT ? "fork out" : "fork in");
      }
      if (now - enteredAt >= REACH_MS + PULL_MS) enter(RETURN);
      break;
    }
    default:
      break;
  }

  if (beepUntil && now >= beepUntil) {
    beepUntil = 0;
    digitalWrite(BUZZER_PIN, LOW);
  }
}