Mini G-code plotter on an Arduino Uno

// Mini G-code interpreter: a two-axis plotter driven line by line from the Serial Monitor.
//
//   X axis: STEP pin 2, DIR pin 5      Y axis: STEP pin 3, DIR pin 6
//   EN pin 8 (LOW enables both A4988s)
//   endstops: X on pin 9, Y on pin 10 (microswitches to GND, LOW when pressed)
//
// The same pins as a GRBL CNC shield. This is not GRBL or Marlin. It is about
// two hundred lines that understand the handful of commands a plotter needs,
// so you can see what a firmware does with each line it is sent:
//
//   G28            home: X, then Y, each toward its endstop (Marlin's meaning;
//                  GRBL homes with $H and uses G28 for a stored position)
//   G0 X.. Y..     move fast to a point            G1 X.. Y.. F..   move at feed F, in mm/min
//   G90 / G91      coordinates absolute / relative M114             report the position
//
// Every line gets "ok" back when it has finished, which is how a sender knows
// to send the next one. G1 moves both motors together with Bresenham's line
// algorithm, so a diagonal is straight and both axes arrive at the same time.
// A real firmware also plans acceleration and looks ahead over many lines; this
// one moves at a steady speed, which is the biggest thing it leaves out.
//
// Mokxi has no carriage: after G28, click each endstop's lever when the
// Serial Monitor says that axis is seeking. Then try:
//   G1 X40 Y30 F1200
//   G91
//   G1 X-10 Y10
//   M114

const int X_STEP = 2, Y_STEP = 3, X_DIR = 5, Y_DIR = 6, ENABLE_PIN = 8;
const int X_LIMIT = 9, Y_LIMIT = 10;

const float STEPS_PER_MM = 5.0;    // full steps: a 200 step motor on a GT2 belt and 20 tooth pulley, 40 mm a turn
const float MAX_MM[2] = {120, 90}; // the plotter's bed
const float RAPID_MM_MIN = 3000;
const float HOMING_MM_MIN = 600;

long position[2] = {0, 0};  // steps
bool homed = false;
bool relative = false;
float feed = 1200;           // mm/min, kept between G1 lines like real G-code

// One step on each axis that is due, then the wait that keeps the feed.
void pulse(bool x, bool y, unsigned long waitUs) __attribute__((noinline));
void pulse(bool x, bool y, unsigned long waitUs) {
  if (x) digitalWrite(X_STEP, HIGH);
  if (y) digitalWrite(Y_STEP, HIGH);
  delayMicroseconds(4);
  digitalWrite(X_STEP, LOW);
  digitalWrite(Y_STEP, LOW);
  delayMicroseconds(waitUs);
}

unsigned long waitFor(float mmPerMin) {
  return (unsigned long)(60000000.0 / (mmPerMin * STEPS_PER_MM));
}

// A straight line to (tx, ty) in steps, both axes together.
void moveLine(long tx, long ty, float mmPerMin) {
  long dx = abs(tx - position[0]), dy = abs(ty - position[1]);
  int sx = tx > position[0] ? 1 : -1, sy = ty > position[1] ? 1 : -1;
  digitalWrite(X_DIR, sx > 0 ? HIGH : LOW);
  digitalWrite(Y_DIR, sy > 0 ? HIGH : LOW);
  long steps = dx > dy ? dx : dy;
  // The feed is along the line, so the axis that moves most sets the pace.
  float length = sqrt((float)dx * dx + (float)dy * dy);
  unsigned long wait = steps > 0 ? (unsigned long)(waitFor(mmPerMin) * length / steps) : 0;
  long errX = steps / 2, errY = steps / 2;
  for (long i = 0; i < steps; i++) {
    errX -= dx;
    errY -= dy;
    bool stepX = errX < 0, stepY = errY < 0;
    if (stepX) { errX += steps; position[0] += sx; }
    if (stepY) { errY += steps; position[1] += sy; }
    pulse(stepX, stepY, wait);
  }
}

bool homeAxis(int axis) {
  int stepPin = axis == 0 ? X_STEP : Y_STEP;
  int limitPin = axis == 0 ? X_LIMIT : Y_LIMIT;
  digitalWrite(axis == 0 ? X_DIR : Y_DIR, LOW);
  Serial.println(axis == 0 ? "homing X: seeking" : "homing Y: seeking");
  long left = (long)((MAX_MM[axis] + 10) * STEPS_PER_MM);
  unsigned long wait = waitFor(HOMING_MM_MIN);
  while (digitalRead(limitPin) == HIGH) {
    if (left-- <= 0) return false;
    pulse(stepPin == X_STEP, stepPin == Y_STEP, wait);
  }
  position[axis] = 0;
  return true;
}

// The number after a letter in a line, or `fallback` when the letter is absent.
float valueAfter(const char *line, char letter, float fallback, bool *found) {
  const char *p = strchr(line, letter);
  if (!p) return fallback;
  if (found) *found = true;
  return atof(p + 1);
}

void report() {
  Serial.print("X:");
  Serial.print(position[0] / STEPS_PER_MM, 2);
  Serial.print(" Y:");
  Serial.println(position[1] / STEPS_PER_MM, 2);
}

void execute(char *line) {
  for (char *c = line; *c; c++) {
    if (*c >= 'a' && *c <= 'z') *c -= 32;  // G-code letters are not case sensitive
  }
  char *comment = strchr(line, ';');
  if (comment) *comment = 0;
  if (line[0] == 0) return;

  int g = -1, m = -1;
  if (line[0] == 'G') g = atoi(line + 1);
  else if (line[0] == 'M') m = atoi(line + 1);

  if (g == 28) {
    if (homeAxis(0) && homeAxis(1)) {
      homed = true;
      Serial.println("homed");
    } else {
      homed = false;
      Serial.println("error: homing failed, no endstop found");
      return;
    }
  } else if (g == 0 || g == 1) {
    if (!homed) {
      Serial.println("error: home first with G28");
      return;
    }
    bool hasX = false, hasY = false, hasF = false;
    float now[2] = {position[0] / STEPS_PER_MM, position[1] / STEPS_PER_MM};
    float x = valueAfter(line, 'X', relative ? 0 : now[0], &hasX);
    float y = valueAfter(line, 'Y', relative ? 0 : now[1], &hasY);
    float f = valueAfter(line, 'F', feed, &hasF);
    if (relative) { x += now[0]; y += now[1]; }
    if (g == 1 && hasF && f > 0) feed = f;
    if (x < 0 || y < 0 || x > MAX_MM[0] || y > MAX_MM[1]) {
      Serial.println("error: outside the bed");
      return;
    }
    moveLine((long)(x * STEPS_PER_MM + 0.5), (long)(y * STEPS_PER_MM + 0.5), g == 0 ? RAPID_MM_MIN : feed);
  } else if (g == 90) {
    relative = false;
  } else if (g == 91) {
    relative = true;
  } else if (m == 114) {
    report();
  } else {
    Serial.print("error: unknown command ");
    Serial.println(line);
    return;
  }
  Serial.println("ok");
}

void setup() {
  Serial.begin(115200);
  pinMode(X_STEP, OUTPUT);
  pinMode(Y_STEP, OUTPUT);
  pinMode(X_DIR, OUTPUT);
  pinMode(Y_DIR, OUTPUT);
  pinMode(ENABLE_PIN, OUTPUT);
  digitalWrite(ENABLE_PIN, LOW);
  pinMode(X_LIMIT, INPUT_PULLUP);
  pinMode(Y_LIMIT, INPUT_PULLUP);
  Serial.println("Mini plotter ready: send G28 to home, then G0 and G1 moves");
}

void loop() {
  static char line[40];
  static int length = 0;
  while (Serial.available() > 0) {
    char c = Serial.read();
    if (c == '\n' || c == '\r') {
      line[length] = 0;
      if (length > 0) execute(line);
      length = 0;
    } else if (length < 39) {
      line[length++] = c;
    }
  }
}