Elevator controller on an Arduino Uno

// Elevator controller: a three-floor lift with call buttons, a cab on a
// servo, a door on a second servo, an obstruction sensor in the doorway and
// a floor display. It remembers every call and serves them in a sensible
// order, and it will never move the cab while the door is open.
//
//   call buttons  floor 1 -> pin 4, floor 2 -> pin 5, floor 3 -> pin 6,
//                 each button's other side to GND (INPUT_PULLUP)
//   door sensor   FC-51 IR sensor across the doorway: OUT -> pin 7, VCC, GND
//   piezo         pin 8 -> piezo -> GND (the arrival ding)
//   cab servo     PWM -> pin 9: 0 degrees is floor 1, 90 floor 2, 180 floor 3
//   door servo    PWM -> pin 10: 0 degrees shut, 90 degrees open
//   display       TM1637 4-digit: CLK -> pin 11, DIO -> pin 12
//
// HOW IT WORKS
//   The loop never waits. Every pass it reads the buttons and the door
//   sensor, then lets the lift take one small step from the state it is in:
//
//     IDLE      parked with the door shut and nothing to do
//     MOVING    the cab glides one degree every CAB_STEP_MS
//     OPENING   the door swings open a little each step
//     OPEN      the door waits DWELL_MS so people can get in and out
//     CLOSING   the door swings shut a little each step
//
//   THE SCHEDULER. Each call sets a flag for its floor. While the lift has
//   calls ahead in the direction it is going, it keeps going and stops at
//   each called floor on the way. When nothing is left ahead it turns
//   round, and when nothing is left at all it parks. Real lifts work the
//   same way (it is called the elevator algorithm, or SCAN): nobody waits
//   while the lift zigzags.
//
//   THE INTERLOCK. stepCab() refuses to move unless the door state is
//   shut AND the door servo is at its shut angle. Every path that would
//   move the cab goes through that one function, so no bug elsewhere can
//   send the cab off with the door open.
//
//   THE OBSTRUCTION SENSOR. While something is in the doorway the door
//   will not start to close, and if it is already closing it opens again.
//
// HOW TO PLAY
//   Press a floor's button to call the cab. Press two or three quickly and
//   watch the order they are served in. While the door is open, drag the
//   door sensor's slider close: the door stays open until the way is clear.
//
// THINGS TO TRY
//   Change DWELL_MS for a patient or an impatient door. Add a fourth floor:
//   a fourth button, FLOORS = 4 and new angles in FLOOR_ANGLE.

#include <Servo.h>
#include <TM1637Display.h>

const int FLOORS = 3;
const int CALL_PIN[FLOORS] = {4, 5, 6};
const int FLOOR_ANGLE[FLOORS] = {0, 90, 180};   // where the cab servo stops for each floor
const int OBSTACLE_PIN = 7;
const int PIEZO_PIN = 8;
const int CAB_PIN = 9;
const int DOOR_PIN = 10;
const int DISPLAY_CLK = 11;
const int DISPLAY_DIO = 12;

const int DOOR_SHUT = 0;
const int DOOR_OPEN = 90;

const unsigned long CAB_STEP_MS = 20;    // one degree per step: 50 degrees a second
const unsigned long DOOR_STEP_MS = 20;   // three degrees per step
const int DOOR_STEP_DEG = 3;
const unsigned long DWELL_MS = 3000;     // how long the door stays open
const unsigned long DEBOUNCE_MS = 25;

enum State { IDLE, MOVING, OPENING, OPEN, CLOSING };
State state = IDLE;
unsigned long stateAt = 0;
unsigned long lastStep = 0;

Servo cab;
Servo door;
TM1637Display display(DISPLAY_CLK, DISPLAY_DIO);

int cabAngle = 0;          // where the cab is, in servo degrees
int doorAngle = DOOR_SHUT;
int floorNow = 0;          // the last floor the cab was at or passed (0 = floor 1)
int direction = 0;         // +1 up, -1 down, 0 none
bool called[FLOORS] = {false, false, false};
bool blockedShown = false; // so "holding open" prints once per obstruction

// Debounce state for each call button.
bool buttonStable[FLOORS];
bool buttonRaw[FLOORS];
unsigned long buttonChangedAt[FLOORS];

void enter(State next, unsigned long now) {
  state = next;
  stateAt = now;
}

bool doorwayBlocked() {
  return digitalRead(OBSTACLE_PIN) == LOW;   // the FC-51 pulls OUT low when it sees someone
}

// ---------- the floor display ----------
// The left digit is an arrow (a cap for up, a cup for down), the right
// digit is the floor. It is only rewritten when something changed.
uint8_t shownSegments[4] = {0xFF, 0xFF, 0xFF, 0xFF};

void showFloor() {
  uint8_t seg[4] = {0, 0, 0, 0};
  if (direction > 0) seg[0] = SEG_A | SEG_B | SEG_F;
  if (direction < 0) seg[0] = SEG_C | SEG_D | SEG_E;
  seg[3] = display.encodeDigit(floorNow + 1);
  if (memcmp(seg, shownSegments, 4) == 0) return;
  memcpy(shownSegments, seg, 4);
  display.setSegments(seg);
}

// ---------- the scheduler ----------
bool callsAbove(int f) {
  for (int i = f + 1; i < FLOORS; i++) if (called[i]) return true;
  return false;
}

bool callsBelow(int f) {
  for (int i = f - 1; i >= 0; i--) if (called[i]) return true;
  return false;
}

// Pick the direction for the next trip: keep going the same way while
// there are calls ahead, otherwise turn round, otherwise stop.
int nextDirection() {
  if (direction >= 0 && callsAbove(floorNow)) return +1;
  if (direction <= 0 && callsBelow(floorNow)) return -1;
  if (callsAbove(floorNow)) return +1;
  if (callsBelow(floorNow)) return -1;
  return 0;
}

// ---------- the interlock ----------
// The only function that moves the cab.
bool stepCab(int dir) {
  if (state != MOVING || doorAngle != DOOR_SHUT) {
    Serial.println("Interlock: door not shut, cab held");
    return false;
  }
  cabAngle += dir;
  cab.write(cabAngle);
  return true;
}

void ding() {
  tone(PIEZO_PIN, 1568, 150);
}

// A button was pressed for floor f.
void call(int f, unsigned long now) {
  Serial.print("Call ");
  Serial.println(f + 1);
  bool here = cabAngle == FLOOR_ANGLE[f] && state != MOVING;
  if (here) {
    // Already here: open the door, or keep it open.
    if (state == IDLE || state == CLOSING) {
      Serial.println("Doors opening");
      enter(OPENING, now);
    } else if (state == OPEN) {
      stateAt = now;   // start the dwell again
    }
    return;
  }
  called[f] = true;
}

void readButtons(unsigned long now) {
  for (int f = 0; f < FLOORS; f++) {
    bool raw = digitalRead(CALL_PIN[f]) == LOW;
    if (raw != buttonRaw[f]) {
      buttonRaw[f] = raw;
      buttonChangedAt[f] = now;
    }
    if (raw != buttonStable[f] && now - buttonChangedAt[f] >= DEBOUNCE_MS) {
      buttonStable[f] = raw;
      if (raw) call(f, now);
    }
  }
}

// Leave a stopped floor: start moving if there is work, else park.
void departOrPark(unsigned long now) {
  direction = nextDirection();
  if (direction == 0) {
    if (state != IDLE) {
      Serial.print("Idle at floor ");
      Serial.println(floorNow + 1);
    }
    enter(IDLE, now);
    return;
  }
  Serial.print("Doors closed, going ");
  Serial.println(direction > 0 ? "up" : "down");
  enter(MOVING, now);
}

void setup() {
  Serial.begin(9600);
  for (int f = 0; f < FLOORS; f++) {
    pinMode(CALL_PIN[f], INPUT_PULLUP);
    buttonStable[f] = false;
    buttonRaw[f] = false;
    buttonChangedAt[f] = 0;
  }
  pinMode(OBSTACLE_PIN, INPUT);
  cab.attach(CAB_PIN);
  door.attach(DOOR_PIN);
  cab.write(cabAngle);
  door.write(doorAngle);
  display.setBrightness(5);
  showFloor();
  Serial.println("Elevator ready at floor 1");
}

void loop() {
  unsigned long now = millis();
  readButtons(now);
  bool blocked = doorwayBlocked();
  if (!blocked) blockedShown = false;

  switch (state) {
    case IDLE:
      if (nextDirection() != 0) departOrPark(now);
      break;

    case MOVING:
      if (now - lastStep >= CAB_STEP_MS) {
        lastStep = now;
        if (!stepCab(direction)) {
          enter(IDLE, now);   // never reached while the code is right
          break;
        }
        // Did the cab just reach a floor?
        for (int f = 0; f < FLOORS; f++) {
          if (cabAngle != FLOOR_ANGLE[f]) continue;
          floorNow = f;
          if (called[f]) {
            called[f] = false;
            Serial.print("Arrived at floor ");
            Serial.println(f + 1);
            ding();
            Serial.println("Doors opening");
            enter(OPENING, now);
          } else {
            Serial.print("Passing floor ");
            Serial.println(f + 1);
          }
        }
        // Safety net: never drive past the top or bottom floor.
        if (state == MOVING && (cabAngle <= FLOOR_ANGLE[0] || cabAngle >= FLOOR_ANGLE[FLOORS - 1])) {
          departOrPark(now);
        }
      }
      break;

    case OPENING:
      if (now - lastStep >= DOOR_STEP_MS) {
        lastStep = now;
        doorAngle = min(doorAngle + DOOR_STEP_DEG, DOOR_OPEN);
        door.write(doorAngle);
        if (doorAngle == DOOR_OPEN) {
          Serial.println("Doors open");
          enter(OPEN, now);
        }
      }
      break;

    case OPEN:
      if (blocked) {
        stateAt = now;   // the dwell starts again once the doorway is clear
        if (!blockedShown) {
          Serial.println("Door blocked: holding open");
          blockedShown = true;
        }
      }
      // Someone at this floor pressed its button again: that is handled in
      // call(). Calls for other floors wait until the door has shut.
      if (now - stateAt >= DWELL_MS) {
        Serial.println("Doors closing");
        enter(CLOSING, now);
      }
      break;

    case CLOSING:
      if (blocked) {
        Serial.println("Obstruction: reopening");
        blockedShown = true;
        enter(OPENING, now);
        break;
      }
      if (now - lastStep >= DOOR_STEP_MS) {
        lastStep = now;
        doorAngle = max(doorAngle - DOOR_STEP_DEG, DOOR_SHUT);
        door.write(doorAngle);
        if (doorAngle == DOOR_SHUT) {
          // Shut: only now may the cab go anywhere.
          departOrPark(now);
        }
      }
      break;
  }

  showFloor();
}