Cánh tay robot 4 servo với ESP32: điều khiển bằng biến trở, ghi nhớ và phát lại động tác

Làm cánh tay robot 4 servo với ESP32: điều khiển từng khớp bằng biến trở, nhấn nút ghi tư thế, phát lại chuỗi động tác mượt, lưu vào flash khi mất điện và gợi ý STEM.

Cánh tay robot là mô hình thu nhỏ của robot công nghiệp trong nhà máy: robot gắp, xếp, hàn, sơn đều lặp lại một chuỗi động tác đã được "dạy" trước. Với ESP32, bốn servo và bốn biến trở, học sinh tự làm được một cánh tay 4 khớp: vặn biến trở để điều khiển từng khớp, nhấn nút để ghi lại từng tư thế, rồi cho cánh tay tự lặp lại cả chuỗi động tác.

Bài viết hướng dẫn cấu tạo, nối dây với nguồn riêng cho servo, chương trình ghi nhớ động tác vào bộ nhớ flash (tắt điện không mất), cách chuyển tư thế mượt và gợi ý hoạt động STEM.

1. Cấu tạo cánh tay 4 khớp

• Đế xoay: quay cả cánh tay sang trái, sang phải.
• Vai và khuỷu: nâng hạ và vươn ra. Hai khớp này chịu lực lớn nhất.
• Kẹp: mở ra, kẹp vật.

Khung có thể làm bằng bìa cứng, mica hoặc gỗ ép, hoặc dùng bộ khung cánh tay robot bằng mica bán sẵn. Cánh tay càng nhẹ thì servo càng ít bị quá tải.

2. Linh kiện cần chuẩn bị

• ESP32 DevKit V1 30 chân.
• 4 servo: SG90 cho đế và kẹp, nên dùng MG90S (bánh răng kim loại) cho vai và khuỷu.
• 4 biến trở 10 kΩ, 2 nút nhấn.
• Nguồn 5 V từ 2 A trở lên cho servo, ví dụ khay 2 pin 18650 qua mạch hạ áp LM2596 chỉnh về 5 V.
• Breadboard 830 lỗ, dây cắm test board.

3. Sơ đồ nối dây

• Không lấy nguồn servo từ cổng USB. Bốn servo cùng chuyển động có thể kéo hơn 1 A, làm ESP32 khởi động lại hoặc hỏng cổng USB của máy tính.
• Nguồn 5 V riêng cấp cho dây đỏ của bốn servo. GND của nguồn này nối chung với GND của ESP32.
• Khi đã nạp xong chương trình, có thể cấp 5 V vào chân VIN để chạy độc lập. Không cắm USB cùng lúc với nguồn VIN.
• Thêm một tụ hóa 1000 µF giữa 5 V và GND ở chỗ cắm dây servo.
• Biến trở nối 3V3 (không nối 5 V), chân giữa vào các chân ADC1: D34, D35, D32, D33.

4. Chương trình ghi nhớ và phát lại

Gói esp32 đã có sẵn thư viện Preferences để lưu dữ liệu vào bộ nhớ flash, không cần cài thêm.

/*
  PYWORLD.VN - Cánh tay robot 4 servo: điều khiển bằng 4 biến trở, ghi nhớ và phát lại động tác (ESP32)
  Servo: đế D23, vai D22, khuỷu D21, kẹp D19 (nguồn 5 V riêng, GND chung). Biến trở: D34, D35, D32, D33
  Nút GHI -> D18, nút CHẠY -> D4 (nút nối xuống GND). Động tác lưu trong bộ nhớ flash, tắt điện không mất
*/

#include <Preferences.h>

const uint8_t SERVO[4] = {23, 22, 21, 19};      // đế xoay, vai, khuỷu, kẹp
const uint8_t BIEN_TRO[4] = {34, 35, 32, 33};
#define NUT_GHI  18
#define NUT_CHAY 4

const int TOI_DA = 20;                           // số tư thế tối đa
uint8_t dongTac[TOI_DA][4];
int soBuoc = 0;
float goc[4] = {90, 90, 90, 90};
Preferences boNho;

void datGoc(int i, float g) {
  g = constrain(g, 0, 180);
  goc[i] = g;
  uint32_t us = 500 + (uint32_t)(1900 * g / 180);
  ledcWrite(SERVO[i], (uint32_t)((uint64_t)us * 65536 / 20000));
}

float docBienTro(int i) {
  long tong = 0;
  for (int k = 0; k < 4; k++) tong += analogRead(BIEN_TRO[i]);
  return (tong / 4) * 180.0f / 4095;
}

// Trả về 1 khi nhả nút sau một lần nhấn ngắn, 2 khi giữ nút quá 2 giây (báo một lần)
int suKienNut(int chan, bool &dangNhan, unsigned long &tNhan, bool &daBaoGiu) {
  bool nhan = (digitalRead(chan) == LOW);
  if (nhan && !dangNhan) { dangNhan = true; tNhan = millis(); daBaoGiu = false; }
  if (nhan && dangNhan && !daBaoGiu && millis() - tNhan > 2000) { daBaoGiu = true; return 2; }
  if (!nhan && dangNhan) {
    dangNhan = false;
    if (!daBaoGiu && millis() - tNhan > 30) return 1;     // bỏ qua dội phím ngắn hơn 30 ms
  }
  return 0;
}

void luuBoNho() {
  boNho.putInt("n", soBuoc);
  boNho.putBytes("dongtac", dongTac, soBuoc * 4);
}

// Đi từ tư thế hiện tại tới tư thế đích trong khoảng 0,8 giây, tăng tốc rồi giảm tốc theo đường cos
bool diToi(const uint8_t dich[4]) {
  float dau[4];
  for (int i = 0; i < 4; i++) dau[i] = goc[i];
  for (int k = 1; k <= 40; k++) {
    float p = (1 - cos(PI * k / 40.0f)) / 2;
    for (int i = 0; i < 4; i++) datGoc(i, dau[i] + (dich[i] - dau[i]) * p);
    delay(20);
    if (digitalRead(NUT_CHAY) == LOW) return false;       // nhấn CHẠY để dừng phát lại
  }
  return true;
}

void phatLai() {
  Serial.printf("Phat lai %d tu the, nhan CHAY de dung\n", soBuoc);
  delay(400);                                             // chờ nhả nút
  while (true) {
    for (int b = 0; b < soBuoc; b++) {
      if (!diToi(dongTac[b])) { Serial.println("Dung phat lai"); delay(400); return; }
      delay(300);
    }
  }
}

void setup() {
  Serial.begin(115200);
  pinMode(NUT_GHI, INPUT_PULLUP);
  pinMode(NUT_CHAY, INPUT_PULLUP);
  for (int i = 0; i < 4; i++) {
    ledcAttach(SERVO[i], 50, 16);
    datGoc(i, docBienTro(i));
  }
  boNho.begin("canhtay", false);
  soBuoc = constrain(boNho.getInt("n", 0), 0, TOI_DA);
  boNho.getBytes("dongtac", dongTac, sizeof(dongTac));
  Serial.printf("Da nap %d tu the tu bo nho\n", soBuoc);
}

void loop() {
  static bool nhanGhi = false, giuGhi = false, nhanChay = false, giuChay = false;
  static unsigned long tGhi = 0, tChay = 0;

  // Chế độ tay: mỗi servo đi theo biến trở của nó, lọc cho đỡ rung
  for (int i = 0; i < 4; i++) {
    float g = goc[i] + 0.2f * (docBienTro(i) - goc[i]);
    if (fabsf(g - goc[i]) > 0.5f) datGoc(i, g);
  }

  int ghi = suKienNut(NUT_GHI, nhanGhi, tGhi, giuGhi);
  if (ghi == 1 && soBuoc < TOI_DA) {
    for (int i = 0; i < 4; i++) dongTac[soBuoc][i] = (uint8_t)lroundf(goc[i]);
    soBuoc++;
    luuBoNho();
    Serial.printf("Ghi tu the %d: de %d, vai %d, khuyu %d, kep %d\n", soBuoc,
                  dongTac[soBuoc - 1][0], dongTac[soBuoc - 1][1], dongTac[soBuoc - 1][2], dongTac[soBuoc - 1][3]);
  } else if (ghi == 2) {
    soBuoc = 0;
    luuBoNho();
    Serial.println("Da xoa het dong tac");
  }

  if (suKienNut(NUT_CHAY, nhanChay, tChay, giuChay) == 1 && soBuoc > 0) {
    phatLai();
  }
  delay(20);
}

Cách dùng:

1. Vặn bốn biến trở để đưa cánh tay tới tư thế thứ nhất, nhấn GHI. Serial Monitor (115200 baud) in bốn góc vừa lưu.
2. Đưa tới tư thế tiếp theo, nhấn GHI. Làm tiếp, tối đa 20 tư thế.
3. Nhấn CHẠY: cánh tay lần lượt đi qua các tư thế đã ghi và lặp lại. Nhấn CHẠY lần nữa để dừng.
4. Giữ GHI 2 giây để xóa hết và ghi lại từ đầu.

5. Giải thích các phần chính

• Chế độ tay: mỗi vòng lặp, góc servo được kéo dần về góc của biến trở (goc + 0,2 × (biến trở − goc)). Servo không bị giật khi tay vặn nhanh, và đứng yên khi biến trở dao động nhẹ.
• Nút nhấn: hàm suKienNut() phân biệt nhấn ngắn và giữ lâu, bỏ qua các lần dội phím ngắn hơn 30 ms.
• Lưu flash: mỗi lần ghi hoặc xóa, số tư thế và mảng góc được lưu bằng putInt(), putBytes(). Lúc khởi động, chương trình nạp lại bằng getInt(), getBytes().
• Chuyển tư thế mượt: mỗi lần chuyển tư thế chia thành 40 bước 20 ms, theo đường cong (1 − cos(π × k / 40)) / 2. Cánh tay khởi động và dừng êm, vật đang kẹp không bị văng ra.

6. Gợi ý dùng trong giờ học STEM

Mục tiêu: học sinh hiểu robot công nghiệp được "dạy" động tác thế nào, làm quen với tọa độ khớp, lưu trữ dữ liệu, và thiết kế cơ khí cân bằng lực.

1. Gắp và đặt. Dạy cánh tay gắp một khối xốp nhỏ từ ô A đặt sang ô B. Thi xem nhóm nào làm được chuỗi động tác ít tư thế nhất mà vẫn chính xác.
2. Đo phạm vi làm việc. Đặt cánh tay trên tờ giấy kẻ ô, đánh dấu các điểm kẹp với tới được, vẽ vùng làm việc. Liên hệ với độ dài hai đoạn tay.
3. Lực và đòn bẩy. Treo vật nặng dần ở đầu kẹp, quan sát khớp vai kêu hoặc tụt trước. Giải thích bằng mô-men lực: cánh tay đòn dài thì mô-men lớn.
4. Mở rộng. Thay biến trở bằng trang web điều khiển qua Wi-Fi như bài xe robot trong cùng loạt, hoặc ghi động tác lên thẻ nhớ.

7. Xử lý lỗi thường gặp

8. Xem thêm

• Lưu dữ liệu vào bộ nhớ flash của ESP32 bằng Preferences
• Đọc nút nhấn, chống dội trên ESP32
• Mạch hạ áp LM2596 và MP1584: chỉnh điện áp và cấp nguồn

Cùng loạt: ESP32 điều khiển servo SG90 bằng PWM, xe robot ESP32 điều khiển qua Wi-Fi.