Simple CNC Machine

#include <Arduino.h>
#include <Servo.h>

const uint8_t X_STEP_PIN = 2;
const uint8_t Y_STEP_PIN = 3;
const uint8_t X_DIR_PIN = 5;
const uint8_t Y_DIR_PIN = 6;
const uint8_t PEN_SERVO_PIN = 9;

const float STEPS_PER_MM = 80.0f;  // GT2 belt, 20-tooth pulley, 1/16 microstepping
const unsigned int STEP_HALF_PERIOD_US = 700;
const int PEN_UP_ANGLE = 25;
const int PEN_DOWN_ANGLE = 70;

Servo penServo;
long xPositionSteps = 0;
long yPositionSteps = 0;
char commandLine[64];
uint8_t commandLength = 0;

void setPen(bool down) {
  penServo.write(down ? PEN_DOWN_ANGLE : PEN_UP_ANGLE);
  delay(350);
}

void pulseStep(uint8_t pin) {
  digitalWrite(pin, HIGH);
  delayMicroseconds(STEP_HALF_PERIOD_US);
  digitalWrite(pin, LOW);
}

void moveToSteps(long targetX, long targetY) {
  long dx = labs(targetX - xPositionSteps);
  long dy = labs(targetY - yPositionSteps);
  int sx = (targetX >= xPositionSteps) ? 1 : -1;
  int sy = (targetY >= yPositionSteps) ? 1 : -1;
  long error = dx - dy;

  digitalWrite(X_DIR_PIN, sx > 0 ? HIGH : LOW);
  digitalWrite(Y_DIR_PIN, sy > 0 ? HIGH : LOW);
  delayMicroseconds(5);

  while (xPositionSteps != targetX || yPositionSteps != targetY) {
    long twiceError = 2 * error;
    bool stepX = false;
    bool stepY = false;

    if (twiceError > -dy) {
      error -= dy;
      xPositionSteps += sx;
      stepX = true;
    }
    if (twiceError < dx) {
      error += dx;
      yPositionSteps += sy;
      stepY = true;
    }

    if (stepX) digitalWrite(X_STEP_PIN, HIGH);
    if (stepY) digitalWrite(Y_STEP_PIN, HIGH);
    delayMicroseconds(STEP_HALF_PERIOD_US);
    if (stepX) digitalWrite(X_STEP_PIN, LOW);
    if (stepY) digitalWrite(Y_STEP_PIN, LOW);
    delayMicroseconds(STEP_HALF_PERIOD_US);
  }
}

bool readAxis(const char *line, char axis, float &value) {
  const char *found = strchr(line, axis);
  if (found == nullptr) return false;
  char *endPointer;
  value = strtod(found + 1, &endPointer);
  return endPointer != found + 1;
}

void executeCommand(char *line) {
  while (*line == ' ' || *line == '\t') line++;
  for (char *p = line; *p; ++p) *p = toupper(*p);

  if (strncmp(line, "M3", 2) == 0) {
    setPen(true);
    Serial.println(F("ok pen down"));
    return;
  }
  if (strncmp(line, "M5", 2) == 0) {
    setPen(false);
    Serial.println(F("ok pen up"));
    return;
  }
  if (strncmp(line, "G92", 3) == 0) {
    float mm;
    if (readAxis(line, 'X', mm)) xPositionSteps = lround(mm * STEPS_PER_MM);
    if (readAxis(line, 'Y', mm)) yPositionSteps = lround(mm * STEPS_PER_MM);
    Serial.println(F("ok coordinate set"));
    return;
  }
  if (strncmp(line, "G0", 2) == 0 || strncmp(line, "G1", 2) == 0) {
    float xMm = xPositionSteps / STEPS_PER_MM;
    float yMm = yPositionSteps / STEPS_PER_MM;
    readAxis(line, 'X', xMm);
    readAxis(line, 'Y', yMm);
    moveToSteps(lround(xMm * STEPS_PER_MM), lround(yMm * STEPS_PER_MM));
    Serial.print(F("ok X"));
    Serial.print(xMm, 2);
    Serial.print(F(" Y"));
    Serial.println(yMm, 2);
    return;
  }
  if (strncmp(line, "HELP", 4) == 0) {
    Serial.println(F("G0/G1 Xmm Ymm | M3 pen down | M5 pen up | G92 X0 Y0"));
    return;
  }
  Serial.println(F("error: use HELP"));
}

void setup() {
  pinMode(X_STEP_PIN, OUTPUT);
  pinMode(Y_STEP_PIN, OUTPUT);
  pinMode(X_DIR_PIN, OUTPUT);
  pinMode(Y_DIR_PIN, OUTPUT);
  digitalWrite(X_STEP_PIN, LOW);
  digitalWrite(Y_STEP_PIN, LOW);

  penServo.attach(PEN_SERVO_PIN);
  setPen(false);

  Serial.begin(115200);
  Serial.println(F("Mini plotter ready. Position axes manually, then send G92 X0 Y0."));
  Serial.println(F("Send HELP for commands."));
}

void loop() {
  while (Serial.available() > 0) {
    char received = (char)Serial.read();
    if (received == '\r' || received == '\n') {
      if (commandLength > 0) {
        commandLine[commandLength] = '\0';
        executeCommand(commandLine);
        commandLength = 0;
      }
    } else if (commandLength < sizeof(commandLine) - 1) {
      commandLine[commandLength++] = received;
    } else {
      commandLength = 0;
      Serial.println(F("error: command too long"));
    }
  }
  delay(1);
}