XY pen plotter on an Arduino Uno

// XY pen plotter: two NEMA 17 steppers on A4988 drivers move a pen over the
// paper, a small servo lifts the pen and puts it down, and two end switches
// tell the plotter where the corner of the paper is. Press START and it draws
// a square, then a five-pointed star, from a list of points, counting every
// step and showing how far along it is on the LCD.
//
//   X driver  STEP -> 3,  DIR -> 4,  EN -> 8
//   Y driver  STEP -> 11, DIR -> 12, EN -> 10
//             on both: EN LOW = motor powered, RST -> SLP,
//             MS1, MS2, MS3 -> GND (full steps), VMOT -> 12 V, VDD -> 5 V
//   pen servo PWM -> 9, VCC -> 5 V, GND -> GND
//   X end switch  NO -> A0, COM -> GND   (INPUT_PULLUP, pressed reads LOW)
//   Y end switch  NO -> A1, COM -> GND
//   START button  -> 2 and GND
//   LCD 16x2 I2C  SDA -> A4, SCL -> A5 (address 0x27)
//
// The numbers. A NEMA 17 takes 200 full steps a turn, and a 20 tooth GT2
// pulley moves the belt 40 mm a turn, so one millimeter is 5 steps. The
// points below are in millimeters from the corner the end switches find.
//
// How it moves in a straight line. To go from A to B the plotter takes as
// many "ticks" as the longer side of the move needs steps. On each tick it
// works out where the pen should be by now (a simple fraction of the way)
// and steps each motor that is behind. A diagonal steps both motors, a flat
// line only one. No delay() in the main flow: micros() says when the next
// step is due, so the plotter still reads its switches between steps.
//
// Safety. Once homed, the pen never goes near the end switches again, so a
// switch pressed while drawing means something is wrong (a jam, a hand, a
// loose belt). The plotter lifts the pen, powers the motors off and waits.
// Press START to home again.
//
// Things to try. Add your own shape to the SHAPES table: a house, your
// initials. Change DRAW_US to draw faster or slower.

#include <Servo.h>
#include <LiquidCrystal_I2C.h>

// ---- pins ----
const int START_PIN = 2;
const int X_STEP = 3, X_DIR = 4;
const int Y_STEP = 11, Y_DIR = 12;
const int X_ENABLE = 8, Y_ENABLE = 10;   // LOW powers a driver
const int PEN_PIN = 9;
const int X_LIMIT = A0, Y_LIMIT = A1;    // analog pins work as digital ones too

// ---- the machine ----
const long STEPS_PER_MM = 5;
const int PEN_UP = 90, PEN_DOWN = 30;   // servo angles
const unsigned long PEN_MS = 300;       // time for the pen to settle
const unsigned long DRAW_US = 8000;     // between steps with the pen down
const unsigned long TRAVEL_US = 4000;   // with the pen up
const unsigned long HOME_US = 5000;     // while it looks for a switch
const long HOME_MAX_STEPS = 120 * STEPS_PER_MM;  // further than the bed
const long BACKOFF_STEPS = 3 * STEPS_PER_MM;     // clear of the switch

// ---- the drawings: points in mm. The pen travels up to the first point
// of each shape, then goes down and draws through the rest. ----
struct Point { int x; int y; };

const Point SQUARE[] = {{5, 5}, {35, 5}, {35, 35}, {5, 35}, {5, 5}};
const Point STAR[] = {{60, 37}, {51, 10}, {74, 27}, {46, 27}, {69, 10}, {60, 37}};

struct Shape { const char *name; const Point *points; int count; };
const Shape SHAPES[] = {
  {"Square", SQUARE, 5},
  {"Star", STAR, 6},
};
const int SHAPE_COUNT = 2;

Servo pen;
LiquidCrystal_I2C lcd(0x27, 16, 2);

// ---- what the plotter is doing ----
enum State { HOMING_X, HOMING_Y, READY, PEN_WAIT, MOVING, FAULT };
State state = HOMING_X;

long posX = 0, posY = 0;          // where the pen is, in steps
long stepsX = 0, stepsY = 0;      // every step taken while drawing
long homeSteps = 0;               // steps taken looking for a switch
unsigned long lastStepUs = 0;
unsigned long penSince = 0;
bool penIsDown = false;

// the move in progress: from (startX, startY), by (dx, dy), in `ticks` ticks
long startX, startY, dx, dy, ticks, tick;
unsigned long stepUs = DRAW_US;

int shapeIndex = 0, pointIndex = 0;
bool parking = false;             // the last move: pen up, back to 0,0
int movesDone = 0;
int totalMoves = 0;               // worked out in setup()

// START, debounced
bool buttonStable = HIGH, buttonLast = HIGH;
unsigned long buttonChanged = 0;

// ---- small helpers ----
void motors(bool on) {
  digitalWrite(X_ENABLE, on ? LOW : HIGH);
  digitalWrite(Y_ENABLE, on ? LOW : HIGH);
}

void setPen(bool down) {
  penIsDown = down;
  pen.write(down ? PEN_DOWN : PEN_UP);
  penSince = millis();
}

// One pulse on a driver's STEP pin moves its motor 1.8 degrees.
void pulse(int pin) {
  digitalWrite(pin, HIGH);
  delayMicroseconds(4);
  digitalWrite(pin, LOW);
}

void stepX(int dir) {
  digitalWrite(X_DIR, dir > 0 ? HIGH : LOW);
  pulse(X_STEP);
  posX += dir;
}

void stepY(int dir) {
  digitalWrite(Y_DIR, dir > 0 ? HIGH : LOW);
  pulse(Y_STEP);
  posY += dir;
}

bool pressed(int pin) { return digitalRead(pin) == LOW; }

void show(const char *top, const char *bottom) {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(top);
  lcd.setCursor(0, 1);
  lcd.print(bottom);
}

void showProgress() {
  int percent = movesDone * 100 / totalMoves;
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(parking ? "Parking" : SHAPES[shapeIndex].name);
  lcd.setCursor(12, 0);
  lcd.print(percent);
  lcd.print("%");
  lcd.setCursor(0, 1);
  lcd.print("steps ");
  lcd.print(stepsX + stepsY);
}

void fault(const char *why) {
  setPen(false);
  motors(false);
  state = FAULT;
  Serial.print("FAULT: ");
  Serial.println(why);
  Serial.println("Motors off. Press START to home again");
  show("FAULT", why);
}

void startHoming() {
  motors(true);
  setPen(false);
  homeSteps = 0;
  state = HOMING_X;
  Serial.println("Homing X");
  show("Homing...", "");
}

// Begin a straight move to a point, in mm.
void beginMove(int xmm, int ymm) {
  startX = posX;
  startY = posY;
  dx = xmm * STEPS_PER_MM - posX;
  dy = ymm * STEPS_PER_MM - posY;
  ticks = max(labs(dx), labs(dy));
  tick = 0;
  stepUs = penIsDown ? DRAW_US : TRAVEL_US;
  state = MOVING;
}

// Decide what comes next: a pen change, a line, the trip home, or done.
void nextMove() {
  if (parking) {
    parking = false;
    motors(false);
    state = READY;
    Serial.print("Done! X steps ");
    Serial.print(stepsX);
    Serial.print(", Y steps ");
    Serial.println(stepsY);
    show("Done! START=again", "");
    lcd.setCursor(0, 1);
    lcd.print("steps ");
    lcd.print(stepsX + stepsY);
    return;
  }
  if (pointIndex == SHAPES[shapeIndex].count) {
    Serial.print(SHAPES[shapeIndex].name);
    Serial.println(" finished");
    shapeIndex++;
    pointIndex = 0;
    if (shapeIndex == SHAPE_COUNT) {
      // everything is drawn: lift the pen and go back to the corner
      parking = true;
      shapeIndex = SHAPE_COUNT - 1;
      if (penIsDown) { setPen(false); state = PEN_WAIT; return; }
      beginMove(0, 0);
      return;
    }
    Serial.print("Drawing ");
    Serial.println(SHAPES[shapeIndex].name);
  }
  // the first point of a shape is reached with the pen up
  bool wantDown = pointIndex > 0;
  if (wantDown != penIsDown) {
    setPen(wantDown);
    state = PEN_WAIT;
    return;
  }
  const Point &p = SHAPES[shapeIndex].points[pointIndex];
  beginMove(p.x, p.y);
}

void setup() {
  Serial.begin(9600);
  pinMode(START_PIN, INPUT_PULLUP);
  pinMode(X_LIMIT, INPUT_PULLUP);
  pinMode(Y_LIMIT, INPUT_PULLUP);
  pinMode(X_STEP, OUTPUT);
  pinMode(X_DIR, OUTPUT);
  pinMode(Y_STEP, OUTPUT);
  pinMode(Y_DIR, OUTPUT);
  pinMode(X_ENABLE, OUTPUT);
  pinMode(Y_ENABLE, OUTPUT);
  pen.attach(PEN_PIN);
  lcd.init();
  lcd.backlight();

  // every point is one move, plus one more to park at the end
  for (int s = 0; s < SHAPE_COUNT; s++) totalMoves += SHAPES[s].count;
  totalMoves += 1;

  Serial.println("XY plotter");
  startHoming();
}

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

  // START, debounced: act once, on the press
  bool reading = digitalRead(START_PIN);
  bool startPressed = false;
  if (reading != buttonLast) { buttonLast = reading; buttonChanged = now; }
  if (now - buttonChanged > 30 && reading != buttonStable) {
    buttonStable = reading;
    if (buttonStable == LOW) startPressed = true;
  }

  // the interlock: an end switch pressed while drawing stops everything
  if (state == MOVING || state == PEN_WAIT) {
    if (pressed(X_LIMIT)) { fault("X limit hit"); return; }
    if (pressed(Y_LIMIT)) { fault("Y limit hit"); return; }
  }

  switch (state) {
    case HOMING_X:
    case HOMING_Y: {
      bool xAxis = state == HOMING_X;
      if (micros() - lastStepUs < HOME_US) break;
      lastStepUs = micros();
      if (pressed(xAxis ? X_LIMIT : Y_LIMIT)) {
        // found it: back off a little and call that spot zero
        for (long i = 0; i < BACKOFF_STEPS; i++) {
          if (xAxis) stepX(+1); else stepY(+1);
          delayMicroseconds(2000);
        }
        Serial.print(xAxis ? "X" : "Y");
        Serial.print(" home after ");
        Serial.print(homeSteps);
        Serial.println(" steps");
        homeSteps = 0;
        if (xAxis) {
          posX = 0;
          state = HOMING_Y;
          Serial.println("Homing Y");
        } else {
          posY = 0;
          motors(false);
          state = READY;
          Serial.println("Ready. Press START to draw");
          show("Ready", "Press START");
        }
        break;
      }
      if (homeSteps >= HOME_MAX_STEPS) {
        fault(xAxis ? "no X switch" : "no Y switch");
        break;
      }
      if (xAxis) stepX(-1); else stepY(-1);
      homeSteps++;
      break;
    }

    case READY:
      if (startPressed) {
        if (pressed(X_LIMIT) || pressed(Y_LIMIT)) {
          Serial.println("An end switch is pressed: clear it first");
          break;
        }
        motors(true);
        stepsX = stepsY = 0;
        shapeIndex = pointIndex = 0;
        movesDone = 0;
        parking = false;
        Serial.print("Drawing ");
        Serial.println(SHAPES[0].name);
        showProgress();
        nextMove();
      }
      break;

    case PEN_WAIT:
      if (now - penSince >= PEN_MS) {
        if (parking) beginMove(0, 0); else nextMove();
      }
      break;

    case MOVING:
      if (micros() - lastStepUs < stepUs) break;
      lastStepUs = micros();
      if (tick < ticks) {
        tick++;
        // where the pen should be after this tick, a fraction of the way
        long wantX = startX + dx * tick / ticks;
        long wantY = startY + dy * tick / ticks;
        if (wantX != posX) { stepX(wantX > posX ? 1 : -1); stepsX++; }
        if (wantY != posY) { stepY(wantY > posY ? 1 : -1); stepsY++; }
      } else {
        // the move is finished
        movesDone++;
        if (!parking) {
          Serial.print("Move ");
          Serial.print(movesDone);
          Serial.print("/");
          Serial.print(totalMoves);
          Serial.print(" at ");
          Serial.print(posX / STEPS_PER_MM);
          Serial.print(",");
          Serial.print(posY / STEPS_PER_MM);
          Serial.println(" mm");
          pointIndex++;
        }
        showProgress();
        nextMove();
      }
      break;

    case FAULT:
      if (startPressed) startHoming();
      break;
  }
}