Tự làm máy vẽ (pen plotter) 2 trục bằng ESP32 và động cơ bước, tự viết bộ đọc G-code

Tự làm máy vẽ 2 trục với ESP32, hai A4988, NEMA 17 và servo nhấc bút; chương trình tự đọc G-code G0 G1 G2 G3, nội suy Bresenham có tăng tốc, script Python gửi file G-code.

Máy vẽ (pen plotter) là máy CNC đơn giản nhất: hai trục X, Y kéo một cây bút trên tờ giấy, một servo nhỏ nhấc bút lên khi di chuyển và hạ xuống khi vẽ. Máy vẽ không có tia laser, không có dao quay nên an toàn để học, dạy STEM và thử nghiệm firmware.

Bài này hướng dẫn tự làm máy vẽ 2 trục với ESP32, hai driver A4988 và hai động cơ NEMA 17, và quan trọng hơn: tự viết chương trình đọc G-code trên ESP32 thay vì dùng firmware có sẵn. Chương trình khoảng 280 dòng, hiểu các lệnh G0, G1, G2, G3, G90, G91, G92, G28, M3, M5, đi thẳng bằng thuật toán Bresenham có tăng tốc, giảm tốc, và trả lời ok sau mỗi dòng giống GRBL. Sơ đồ nối giống hệt máy laser FluidNC ở bài 13 của loạt, nên cùng một mạch có thể đổi qua lại giữa hai firmware.

1. Cấu tạo và linh kiện

• ESP32 DevKit V1 30 chân, dây USB có dây dữ liệu.
• Hai driver A4988 (hoặc TMC2209 để chạy êm), hai động cơ bước NEMA 17.
• Khung: nhôm định hình 2020 với bánh xe V hoặc thanh trượt tròn, dây đai GT2 và puly 20 răng (bài 4, 8 của loạt). Khung nhỏ cỡ A5 (200 × 150 mm) là đủ để bắt đầu.
• Servo SG90 nhấc bút, ống giữ bút in 3D hoặc tự chế.
• Hai công tắc hành trình ở đầu âm của X và Y (không bắt buộc).
• Nguồn 12 V cho driver; mạch hạ áp MP1584 hoặc LM2596 lấy 5 V cho servo.
• Breadboard, dây cắm.

2. Sơ đồ nối

• A4988: RST nối SLP, VDD lấy 3V3 của ESP32, VMOT lấy 12 V có tụ lọc, chỉnh VREF theo động cơ trước khi chạy (bài 7).
• Servo lấy 5 V từ mạch hạ áp, không lấy từ chân 3V3 hay chân 5V của ESP32 khi cắm USB, vì servo có thể kéo dòng lớn lúc quay.

3. Chương trình

Bài dùng Arduino IDE 2 với gói esp32 phiên bản 3.3.10; chương trình chỉ dùng thư viện có sẵn trong gói, không cần cài thêm. Chọn bo DOIT ESP32 DEVKIT V1, cổng COM tương ứng rồi nạp.

/*
  PYWORLD.VN - Máy vẽ 2 trục bằng ESP32: tự viết bộ đọc G-code (G0 G1 G2 G3 G4 G21 G28 G90 G91 G92, M3 M5 M17 M18, $H, ?)
  Driver A4988: X STEP GPIO26, X DIR GPIO16, Y STEP GPIO27, Y DIR GPIO17, EN chung GPIO13 (mức thấp = bật driver)
  Công tắc về gốc X GPIO21, Y GPIO22 (nối về GND), servo nhấc bút GPIO25. Serial 115200, mỗi dòng trả về ok hoặc error
*/
#include <Arduino.h>

// ---- chân nối (giống sơ đồ trong bài FluidNC của loạt) ----
const int X_STEP = 26, X_DIR = 16;
const int Y_STEP = 27, Y_DIR = 17;
const int EN_PIN = 13;
const int X_GIOI_HAN = 21, Y_GIOI_HAN = 22;
const int SERVO_PIN = 25;

// ---- thông số máy ----
const float BUOC_MM = 80.0;                   // dây đai GT2, puly 20 răng, vi bước 1/16
const float MAY_X = 200.0, MAY_Y = 150.0;     // vùng vẽ (mm)
const float TOC_TOI_DA = 6000.0;              // mm/phút, cũng là tốc độ của G0
const float TOC_KHOI_DONG = 300.0;            // mm/phút, tốc độ đầu và cuối mỗi đoạn
const float GIA_TOC = 1500.0;                 // mm/s²
const float DOAN_CUNG = 0.5;                  // cung tròn được chia thành các đoạn thẳng dài 0,5 mm
const int BUT_NHAC_US = 1200, BUT_HA_US = 1700;   // độ rộng xung servo khi nhấc, hạ bút (µs)
const bool DAO_X = false, DAO_Y = false;          // đổi thành true nếu trục chạy ngược chiều

long buocX = 0, buocY = 0;      // vị trí hiện tại tính bằng bước (bật máy coi là gốc 0, 0)
float tocDo = 1500.0;           // F hiện tại (mm/phút)
bool tuyetDoi = true;           // G90 hay G91
int cheDo = 0;                  // lệnh chuyển động duy trì: 0, 1, 2, 3
bool butHa = false;

struct Lenh {
  int g[4];
  int soG = 0;
  int m = -1;
  bool coX = false, coY = false, coZ = false, coF = false, coP = false;
  float x = 0, y = 0, z = 0, f = 0, i = 0, j = 0, p = 0;
};

void datServo(int us) {
  // LEDC 50 Hz, 16 bit: một chu kỳ 20 000 µs ứng với 65535
  ledcWrite(SERVO_PIN, (uint32_t)((uint64_t)us * 65535 / 20000));
}

void dieuKhienBut(bool ha) {
  if (ha == butHa) return;
  datServo(ha ? BUT_HA_US : BUT_NHAC_US);
  butHa = ha;
  delay(250);                   // chờ servo quay tới nơi
}

float viTriX() { return buocX / BUOC_MM; }
float viTriY() { return buocY / BUOC_MM; }

void xung(int chan) {
  digitalWrite(chan, HIGH);
  delayMicroseconds(3);         // A4988 cần xung STEP rộng ít nhất 1 µs
  digitalWrite(chan, LOW);
}

// Đi thẳng tới (x, y) mm với tốc độ f mm/phút.
// Bresenham chia bước cho hai trục; tốc độ tăng dần ở đầu đoạn, giảm dần ở cuối đoạn (hình thang).
void diThang(float x, float y, float f) {
  x = constrain(x, 0.0f, MAY_X);
  y = constrain(y, 0.0f, MAY_Y);
  long dichX = lroundf(x * BUOC_MM), dichY = lroundf(y * BUOC_MM);
  long dx = dichX - buocX, dy = dichY - buocY;
  long nx = labs(dx), ny = labs(dy);
  long tong = max(nx, ny);
  if (tong == 0) return;
  digitalWrite(X_DIR, ((dx >= 0) != DAO_X) ? HIGH : LOW);
  digitalWrite(Y_DIR, ((dy >= 0) != DAO_Y) ? HIGH : LOW);
  float dai = sqrtf((float)dx * dx + (float)dy * dy) / BUOC_MM;   // chiều dài đoạn (mm)
  float mmMoiLan = dai / tong;                                     // quãng đường mỗi vòng lặp
  float vMax = constrain(f, TOC_KHOI_DONG, TOC_TOI_DA) / 60.0;     // mm/s
  float v0 = TOC_KHOI_DONG / 60.0;
  float daiTang = (vMax * vMax - v0 * v0) / (2 * GIA_TOC);         // quãng đường tăng tốc
  if (2 * daiTang > dai) daiTang = dai / 2;
  long saiX = tong / 2, saiY = tong / 2;
  unsigned long moc = micros();
  for (long k = 0; k < tong; k++) {
    float daDi = k * mmMoiLan;
    float s = min(daDi, dai - daDi);                               // khoảng cách tới đầu hoặc cuối đoạn
    float v = (s < daiTang) ? sqrtf(v0 * v0 + 2 * GIA_TOC * s) : vMax;
    if (v > vMax) v = vMax;
    unsigned long chuKy = (unsigned long)(mmMoiLan / v * 1e6);     // µs cho vòng lặp này
    saiX -= nx;
    saiY -= ny;
    if (saiX < 0) { saiX += tong; digitalWrite(X_STEP, HIGH); }
    if (saiY < 0) { saiY += tong; digitalWrite(Y_STEP, HIGH); }
    delayMicroseconds(3);
    digitalWrite(X_STEP, LOW);
    digitalWrite(Y_STEP, LOW);
    while (micros() - moc < chuKy) { }
    moc += chuKy;
  }
  buocX = dichX;
  buocY = dichY;
}

// Cung tròn G2 (cw = true) hoặc G3 tới (x, y); tâm cách điểm đầu (i, j). Điểm cuối trùng điểm đầu là cả vòng tròn.
void diCung(float x, float y, float i, float j, bool cw, float f) {
  float x0 = viTriX(), y0 = viTriY();
  float cx = x0 + i, cy = y0 + j;
  float r = sqrtf(i * i + j * j);
  float a0 = atan2f(y0 - cy, x0 - cx), a1 = atan2f(y - cy, x - cx);
  float quet = a1 - a0;
  if (cw && quet >= -1e-6f) quet -= 2 * PI;
  if (!cw && quet <= 1e-6f) quet += 2 * PI;
  int n = max(1, (int)ceilf(fabsf(quet) * r / DOAN_CUNG));
  for (int k = 1; k <= n; k++) {
    if (k == n) {
      diThang(x, y, f);
    } else {
      float a = a0 + quet * k / n;
      diThang(cx + r * cosf(a), cy + r * sinf(a), f);
    }
  }
}

// Về gốc một trục: chạy chậm về phía âm tới khi công tắc báo chạm (mức thấp), rồi lùi ra 2 mm
bool veGocTruc(int chanStep, int chanDir, int congTac, bool dao) {
  digitalWrite(chanDir, dao ? HIGH : LOW);            // chiều âm
  long toiDa = lroundf((max(MAY_X, MAY_Y) + 20) * BUOC_MM);
  long dem = 0;
  while (digitalRead(congTac) == HIGH) {
    if (++dem > toiDa) return false;                  // đi hết hành trình mà chưa chạm
    xung(chanStep);
    delayMicroseconds(1000);                          // khoảng 750 mm/phút
  }
  digitalWrite(chanDir, dao ? LOW : HIGH);
  for (long k = 0; k < lroundf(2 * BUOC_MM); k++) {
    xung(chanStep);
    delayMicroseconds(1000);
  }
  return true;
}

// Tách một dòng đã viết hoa, bỏ chú thích, thành các từ G, M, X, Y, Z, F, I, J, P
bool phanTich(const String &s, Lenh &l) {
  int n = s.length(), k = 0;
  while (k < n) {
    char c = s[k];
    if (c == ' ' || c == '\t') { k++; continue; }
    if (c < 'A' || c > 'Z') return false;
    int dau = ++k;
    while (k < n && (isdigit((unsigned char)s[k]) || s[k] == '.' || s[k] == '-' || s[k] == '+')) k++;
    if (k == dau) return false;
    float v = s.substring(dau, k).toFloat();
    switch (c) {
      case 'G': if (l.soG < 4) l.g[l.soG++] = (int)v; break;
      case 'M': l.m = (int)v; break;
      case 'X': l.coX = true; l.x = v; break;
      case 'Y': l.coY = true; l.y = v; break;
      case 'Z': l.coZ = true; l.z = v; break;
      case 'F': l.coF = true; l.f = v; break;
      case 'I': l.i = v; break;
      case 'J': l.j = v; break;
      case 'P': l.coP = true; l.p = v; break;
      default: break;                                 // N (số dòng), S, T…: bỏ qua
    }
  }
  return true;
}

String xuLy(String dong) {
  int vt;
  while ((vt = dong.indexOf('(')) >= 0) {             // bỏ chú thích trong ngoặc
    int ket = dong.indexOf(')', vt);
    if (ket < 0) ket = dong.length() - 1;
    dong.remove(vt, ket - vt + 1);
  }
  vt = dong.indexOf(';');                             // bỏ chú thích sau dấu chấm phẩy
  if (vt >= 0) dong.remove(vt);
  dong.trim();
  dong.toUpperCase();
  if (dong.length() == 0) return "ok";

  if (dong == "?") {
    char buf[80];
    snprintf(buf, sizeof(buf), "<Idle|MPos:%.3f,%.3f|But:%s>", viTriX(), viTriY(), butHa ? "ha" : "nhac");
    Serial.println(buf);
    return "ok";
  }
  if (dong == "$H") {
    dieuKhienBut(false);
    if (!veGocTruc(X_STEP, X_DIR, X_GIOI_HAN, DAO_X) || !veGocTruc(Y_STEP, Y_DIR, Y_GIOI_HAN, DAO_Y)) {
      return "error: khong tim thay cong tac";
    }
    buocX = 0;
    buocY = 0;
    return "ok";
  }

  Lenh l;
  if (!phanTich(dong, l)) return "error: sai cu phap";
  for (int k = 0; k < l.soG; k++) {
    int g = l.g[k];
    if (g >= 0 && g <= 3) cheDo = g;
    else if (g == 4) { if (l.coP) delay((unsigned long)(l.p * 1000)); }
    else if (g == 20) return "error: chi ho tro mm (G21)";
    else if (g == 21 || g == 17) { }
    else if (g == 90) tuyetDoi = true;
    else if (g == 91) tuyetDoi = false;
    else if (g == 92) {                               // đặt vị trí hiện tại thành giá trị mới
      if (l.coX) buocX = lroundf(l.x * BUOC_MM);
      if (l.coY) buocY = lroundf(l.y * BUOC_MM);
      return "ok";
    }
    else if (g == 28) { dieuKhienBut(false); diThang(0, 0, TOC_TOI_DA); return "ok"; }
    else return String("error: khong ho tro G") + g;
  }
  if (l.m >= 0) {
    if (l.m == 3 || l.m == 4) dieuKhienBut(true);    // M3, M4: hạ bút
    else if (l.m == 5 || l.m == 2 || l.m == 30) dieuKhienBut(false);
    else if (l.m == 17) digitalWrite(EN_PIN, LOW);    // bật driver
    else if (l.m == 18 || l.m == 84) digitalWrite(EN_PIN, HIGH);   // tắt driver, đẩy tay được
    else return String("error: khong ho tro M") + l.m;
  }
  if (l.coF) tocDo = l.f;
  if (l.coZ) dieuKhienBut(l.z < 0);                   // kiểu G-code có trục Z: Z âm là hạ bút
  if (l.coX || l.coY) {
    float x = viTriX(), y = viTriY();
    float tx = l.coX ? (tuyetDoi ? l.x : x + l.x) : x;
    float ty = l.coY ? (tuyetDoi ? l.y : y + l.y) : y;
    if (cheDo == 0) diThang(tx, ty, TOC_TOI_DA);
    else if (cheDo == 1) diThang(tx, ty, tocDo);
    else diCung(tx, ty, l.i, l.j, cheDo == 2, tocDo);
  }
  return "ok";
}

String dem;

void setup() {
  Serial.begin(115200);
  pinMode(X_STEP, OUTPUT);
  pinMode(X_DIR, OUTPUT);
  pinMode(Y_STEP, OUTPUT);
  pinMode(Y_DIR, OUTPUT);
  pinMode(EN_PIN, OUTPUT);
  digitalWrite(EN_PIN, LOW);                          // bật hai driver
  pinMode(X_GIOI_HAN, INPUT_PULLUP);
  pinMode(Y_GIOI_HAN, INPUT_PULLUP);
  ledcAttach(SERVO_PIN, 50, 16);
  datServo(BUT_NHAC_US);
  Serial.println("PYWORLD may ve ESP32 san sang. Go ? de xem vi tri.");
}

void loop() {
  while (Serial.available()) {
    char c = Serial.read();
    if (c == '\n' || c == '\r') {
      if (dem.length() > 0) {
        Serial.println(xuLy(dem));
        dem = "";
      }
    } else if (dem.length() < 120) {
      dem += c;
    }
  }
}

4. Giải thích chương trình

Đi thẳng: hàm diThang() và thuật toán Bresenham

• Đổi tọa độ milimét ra số bước (BUOC_MM = 80 bước/mm), tính số bước mỗi trục phải đi.
• Trục đi nhiều bước hơn phát một xung STEP ở mỗi vòng lặp; trục kia dùng biến sai số cộng dồn để biết vòng nào cần bước. Chỉ dùng phép cộng, trừ số nguyên nên ESP32 chạy rất nhanh.
• Thời gian chờ giữa hai vòng lặp tính từ tốc độ: đầu đoạn tăng tốc từ TOC_KHOI_DONG với gia tốc GIA_TOC, cuối đoạn giảm tốc đối xứng (hình thang). Nhờ vậy động cơ không bị mất bước khi khởi động.
• Tọa độ bị giới hạn trong vùng 0–200 mm (X), 0–150 mm (Y) để bút không chạy quá khung.

Cung tròn: hàm diCung()

Tâm cung bằng vị trí hiện tại cộng (I, J), giống cách hiểu của GRBL. Góc quét được tính theo chiều G2 (thuận chiều kim đồng hồ) hay G3, rồi chia thành các đoạn thẳng dài khoảng 0,5 mm và gọi diThang() cho từng đoạn. Điểm cuối trùng điểm đầu thì vẽ cả vòng tròn.

Bộ đọc lệnh: hàm xuLy() và phanTich()

• Bỏ phần chú thích trong ngoặc và sau dấu chấm phẩy, viết hoa toàn dòng.
• Tách thành các cặp chữ, số; một dòng có thể có nhiều lệnh G (ví dụ G21 G90).
• Lệnh chuyển động là lệnh duy trì: dòng chỉ có X40 Y10 vẫn chạy theo G0, G1, G2 hay G3 gần nhất.
• Bút: M3 hạ bút, M5 nhấc bút; file G-code dùng trục Z thì Z âm là hạ bút. Độ rộng xung servo khi nhấc, hạ đặt ở BUT_NHAC_US, BUT_HA_US: chỉnh hai số này cho vừa cơ cấu của bạn.
• $H về gốc bằng công tắc; máy không có công tắc thì đưa bút về góc trái dưới bằng tay trước khi bật điện, hoặc dùng G92 X0 Y0 để đặt vị trí hiện tại là gốc.
• M18 tắt driver để đẩy tay xe trượt, M17 bật lại.

5. Chạy thử bằng Serial Monitor

1. Mở Serial Monitor, chọn 115200 baud và kiểu xuống dòng New Line.
2. Kẹp tờ giấy, đặt bút ở góc trái dưới (hoặc gõ $H nếu có công tắc).
3. Gõ G0 X20 Y20: bút đang nhấc đi tới (20, 20). Trục nào chạy ngược chiều thì đổi DAO_X, DAO_Y thành true.
4. Gõ M3 rồi G1 X80 Y20 F3000: bút hạ xuống và kẻ một đường dài 60 mm. Đo bằng thước; sai lệch nhiều thì xem lại vi bước và BUOC_MM.
5. Gõ M5 để nhấc bút.

6. Vẽ một file G-code

File mẫu dưới đây vẽ khung vuông, đường tròn và ngôi sao năm cánh cùng tâm (50, 50):

(PYWORLD.VN - hinh mau cho may ve: khung vuong, duong tron, ngoi sao)
G21 G90
M5
G0 X20 Y20
M3
G1 X80 Y20 F3000
G1 X80 Y80
G1 X20 Y80
G1 X20 Y20
M5
G0 X75 Y50
M3
G2 X75 Y50 I-25 J0 F2000
M5
G0 X50 Y72
M3
G1 X37.069 Y32.202 F2500
G1 X70.923 Y56.798 F2500
G1 X29.077 Y56.798 F2500
G1 X62.931 Y32.202 F2500
G1 X50 Y72 F2500
M5
G28

Gửi cả file bằng script Python dưới đây: script gửi từng dòng, chờ ESP32 trả ok rồi mới gửi tiếp, dừng lại nếu gặp error. Cài Python 3 và thư viện pyserial (pip install pyserial) trên máy tính, rồi chạy python gui_gcode.py COM5 hinh_mau.nc (thay COM5 bằng cổng của ESP32).

# PYWORLD.VN - Gửi một file G-code tới máy vẽ ESP32: gửi từng dòng, chờ "ok" rồi mới gửi dòng tiếp theo
# Cần Python 3 và thư viện pyserial (pip install pyserial).
# Chạy: python gui_gcode.py COM5 hinh_mau.nc      (Linux, macOS: /dev/ttyUSB0 thay cho COM5)

import sys
import time

import serial


def gui_file(cong, ten_file):
    with serial.Serial(cong, 115200, timeout=30) as may:
        time.sleep(2)                      # ESP32 khởi động lại khi mở cổng: chờ dòng chào
        may.reset_input_buffer()
        with open(ten_file, encoding="utf-8") as f:
            for so_dong, dong in enumerate(f, 1):
                dong = dong.strip()
                if not dong:
                    continue
                may.write((dong + "\n").encode("ascii", errors="ignore"))
                while True:
                    tra_loi = may.readline().decode("ascii", errors="replace").strip()
                    if tra_loi == "":
                        print(f"Dong {so_dong}: het thoi gian cho, dung lai")
                        return
                    if tra_loi == "ok":
                        break
                    if tra_loi.startswith("error"):
                        print(f"Dong {so_dong} ({dong}): {tra_loi}")
                        return
                    print(tra_loi)        # dòng trạng thái hoặc thông báo khác
                print(f"{so_dong}: {dong}")
    print("Xong.")


if __name__ == "__main__":
    if len(sys.argv) != 3:
        print("Cach dung: python gui_gcode.py <cong COM> <file .nc>")
        sys.exit(1)
    gui_file(sys.argv[1], sys.argv[2])

Muốn vẽ hình tự thiết kế, xuất G-code từ phần mềm vẽ vector (ví dụ Inkscape có các tiện ích mở rộng xuất G-code cho máy vẽ) hoặc phần mềm laser ở chế độ khắc nét, với lệnh nhấc, hạ bút là M5, M3 hoặc trục Z. File có lệnh chương trình không hỗ trợ sẽ nhận error ở dòng đó.

7. Mở rộng

• Chạy FluidNC thay chương trình tự viết: mạch giữ nguyên, nạp FluidNC (bài 13) và khai báo servo nhấc bút như một trục Z kiểu servo theo tài liệu FluidNC. Khi đó dùng được giao diện web và các phần mềm gửi G-code của GRBL.
• Đọc file từ thẻ nhớ hoặc nhận G-code qua Wi-Fi bằng web server của ESP32.
• Dạy STEM: học sinh viết G-code bằng tay để vẽ chữ cái, đa giác; hoặc sửa hàm diCung() để vẽ hình xoắn ốc.

8. Xử lý lỗi

9. Xem thêm

• Bài trước trong loạt: Phần mềm cho máy CNC. Bài sau: Tự lắp máy khắc laser diode mini.
• Bài 5 (G-code cơ bản) và bài 13 (ESP32 với FluidNC, cùng sơ đồ nối) của loạt.
• ESP32 điều khiển động cơ bước NEMA 17 với A4988: thử từng động cơ trước khi lắp máy.
• Mạch hạ áp LM2596 và MP1584: lấy nguồn 5 V cho servo.