Điều khiển vị trí động cơ DC bằng PID trên ESP32: encoder, quỹ đạo hình thang, bù vùng chết

Điều khiển vị trí động cơ DC có encoder bằng PID trên ESP32 và L298N: quay đúng góc, quỹ đạo hình thang, bù vùng chết, vùng nghỉ chống rung, lệnh Serial, mô phỏng ba cách và cách chỉnh thông số.

Servo quay tới đúng góc vì bên trong có mạch điều khiển vị trí. Muốn một động cơ DC giảm tốc có encoder làm việc tương tự (quay đúng 90°, đúng 3 vòng, dừng và giữ yên), ESP32 phải tự làm phần đó: đọc góc từ encoder, so với góc đích, tính duty cho driver. Bài này hướng dẫn điều khiển vị trí động cơ DC bằng PID trên ESP32 với driver L298N. Chương trình có ba phần thường bị bỏ qua: quỹ đạo hình thang để động cơ tăng tốc và giảm tốc êm, bù vùng chết để động cơ không dừng lệch đích, vùng nghỉ quanh đích để động cơ không rung. Cuối bài có mô phỏng so sánh ba cách điều khiển và cách chỉnh thông số trên mạch thật.

1. Điều khiển vị trí khác điều khiển tốc độ thế nào

Bài giữ tốc độ động cơ DC bằng PID đo số vòng/phút và giữ cho nó bằng giá trị đặt; động cơ luôn quay, duty luôn lớn hơn ngưỡng để động cơ chạy. Điều khiển vị trí thì khác:

• Đại lượng điều khiển là góc (đếm xung encoder rồi đổi ra độ), không phải tốc độ.
• Tới đích là phải dừng: duty về gần 0. Mà động cơ DC giảm tốc có vùng chết: duty dưới khoảng 15–25 % thì không đủ lực thắng ma sát, động cơ đứng yên. Gần đích, sai số nhỏ, duty tính ra nhỏ nên rơi vào vùng chết: động cơ dừng trước hoặc sau đích vài độ.
• Quán tính gây vọt lố: lệnh nhảy thẳng tới đích làm động cơ chạy hết tốc, tới đích chưa kịp dừng. Cần thành phần D (theo vận tốc) để hãm, và tốt hơn là cho điểm đặt đi dần tới đích theo một quỹ đạo.

2. Phần cứng và nối dây

• ESP32 DevKit V1 30 chân.
• Driver L298N (cách dùng ở bài ESP32 điều khiển động cơ DC với L298N).
• Động cơ DC giảm tốc có encoder hai kênh A, B, ví dụ loại 12 V, encoder 11 xung mỗi vòng trục động cơ, hộp số 1:30. Đếm cả bốn cạnh của hai kênh thì một vòng trục ra là 11 × 4 × 30 = 1320 xung, mỗi xung ứng với khoảng 0,27°.
• Nguồn 12 V cho L298N, GND chung với ESP32.

Động cơ khác thì sửa DEM_MOI_VONG theo số xung encoder và tỉ số hộp số. Cách đếm xung bằng bộ đếm phần cứng PCNT của ESP32 có ở bài đo tốc độ động cơ bằng encoder và PCNT; chương trình dưới đây dùng lại đúng cách cài đặt đó, thêm chế độ cộng dồn để đếm được nhiều vòng.

3. Vòng điều khiển vị trí

Mỗi chu kỳ 10 ms, chương trình làm bốn việc:

1. Đọc số đếm PCNT, đổi ra góc (độ), tính vận tốc và lọc bớt nhiễu.
2. Cho quỹ đạo tiến thêm một bước: điểm đặt không nhảy thẳng tới đích mà đi dần với vận tốc, gia tốc có giới hạn.
3. Tính PID từ sai số giữa điểm đặt của quỹ đạo và góc đo:
   • P tỉ lệ với sai số góc;
   • D tỉ lệ với sai lệch giữa vận tốc quỹ đạo và vận tốc đo, có tác dụng hãm;
   • I cộng dồn sai số khi đã ở gần đích.
4. Cộng thêm duty bù vùng chết theo chiều cần quay, giới hạn trong −100…100 %, xuất ra L298N. Nếu quỹ đạo đã tới đích và sai số nhỏ hơn 0,5° thì ngắt động cơ.

4. Quỹ đạo hình thang

Quỹ đạo hình thang có hai thông số: vận tốc tối đa vMax (độ/s) và gia tốc aMax (độ/s²). Thời gian tăng tốc là vMax / aMax, quãng đường tăng tốc là vMax² / (2 × aMax). Với 360 độ/s và 1500 độ/s²: tăng tốc 0,24 s, đi được 43,2°. Lệnh ngắn hơn 2 × 43,2 ≈ 86° thì không kịp lên vMax, quỹ đạo thành hình tam giác.

Cách tính trong hàm buocQuyDao() không cần biết trước thời điểm bắt đầu giảm tốc. Mỗi bước, nó tính vận tốc lớn nhất mà từ đó còn kịp dừng đúng đích với gia tốc aMax, tức √(2 × aMax × quãng còn lại). Vận tốc đặt được tăng hoặc giảm về phía giá trị đó, mỗi bước thay đổi không quá aMax × 10 ms. Vì vận tốc chỉ đổi theo từng bước rời rạc, công thức trừ thêm aMax × DT / 2; không có phần trừ này, quỹ đạo vọt qua đích khoảng 1,8° rồi quay lại. Cách tính này cũng đúng khi đổi đích giữa chừng: quỹ đạo tự giảm tốc, đổi chiều.

5. Chương trình

/*
  PYWORLD.VN - Điều khiển vị trí (góc quay) động cơ DC có encoder bằng PID trên ESP32, công suất qua L298N
  PYWORLD.VN - Quỹ đạo hình thang giới hạn tốc độ, gia tốc; bù vùng chết; vùng nghỉ quanh đích chống rung
  PYWORLD.VN - Lệnh (115200, New Line): g 90 = tới góc 90 độ, r -45 = quay thêm -45 độ, z = lấy vị trí hiện tại làm 0,
               v 360 = tốc độ tối đa độ/s, a 1500 = gia tốc độ/s2, p 4, i 0, d 0.15 = đổi hệ số, b 20 = duty bù vùng chết
*/

#include "driver/pulse_cnt.h"

#define ENC_A   32
#define ENC_B   33
#define ENA_PIN 27
#define IN1_PIN 26
#define IN2_PIN 25

const float DEM_MOI_VONG = 11 * 4 * 30;   // xung mỗi vòng x 4 cạnh x tỉ số hộp số: sửa theo động cơ
const float DT = 0.01f;                    // chu kỳ điều khiển 10 ms
const float LOC = 0.3f;                    // hệ số lọc vận tốc đo (0..1)
const float VUNG_NGHI = 0.5f;              // sai số nhỏ hơn mức này (độ) sau khi tới đích thì ngắt động cơ
const float VUNG_TICH_PHAN = 10.0f;        // chỉ cộng tích phân khi sai số nhỏ hơn mức này (độ)

float Kp = 4.0f, Ki = 0.0f, Kd = 0.15f;    // Kp: % duty trên độ; Kd: % duty trên độ/s
float dutyBu = 20.0f;                       // % duty nhỏ nhất làm động cơ bắt đầu quay (đo theo hướng dẫn trong bài)
float vMax = 360.0f, aMax = 1500.0f;        // giới hạn quỹ đạo: độ/s, độ/s2

float mucTieu = 0;                          // góc đích (độ)
float datQD = 0, vQD = 0;                   // vị trí và vận tốc của quỹ đạo tại thời điểm hiện tại
float tichPhan = 0, viTriTruoc = 0, vanTocLoc = 0, duty = 0;
int demGoc = 0;                             // số đếm ứng với góc 0
pcnt_unit_handle_t donVi = NULL;

void caiDatPCNT() {
  pcnt_unit_config_t cauHinh = {};
  cauHinh.high_limit = 30000;
  cauHinh.low_limit = -30000;
  cauHinh.flags.accum_count = 1;           // cộng dồn khi chạm giới hạn: đếm được nhiều vòng
  ESP_ERROR_CHECK(pcnt_new_unit(&cauHinh, &donVi));
  pcnt_glitch_filter_config_t locNhieu = {};
  locNhieu.max_glitch_ns = 1000;
  ESP_ERROR_CHECK(pcnt_unit_set_glitch_filter(donVi, &locNhieu));
  pcnt_chan_config_t kA = {};
  kA.edge_gpio_num = ENC_A;
  kA.level_gpio_num = ENC_B;
  pcnt_channel_handle_t kenhA;
  ESP_ERROR_CHECK(pcnt_new_channel(donVi, &kA, &kenhA));
  pcnt_chan_config_t kB = {};
  kB.edge_gpio_num = ENC_B;
  kB.level_gpio_num = ENC_A;
  pcnt_channel_handle_t kenhB;
  ESP_ERROR_CHECK(pcnt_new_channel(donVi, &kB, &kenhB));
  pcnt_channel_set_edge_action(kenhA, PCNT_CHANNEL_EDGE_ACTION_DECREASE, PCNT_CHANNEL_EDGE_ACTION_INCREASE);
  pcnt_channel_set_level_action(kenhA, PCNT_CHANNEL_LEVEL_ACTION_KEEP, PCNT_CHANNEL_LEVEL_ACTION_INVERSE);
  pcnt_channel_set_edge_action(kenhB, PCNT_CHANNEL_EDGE_ACTION_INCREASE, PCNT_CHANNEL_EDGE_ACTION_DECREASE);
  pcnt_channel_set_level_action(kenhB, PCNT_CHANNEL_LEVEL_ACTION_KEEP, PCNT_CHANNEL_LEVEL_ACTION_INVERSE);
  pcnt_unit_add_watch_point(donVi, cauHinh.high_limit);
  pcnt_unit_add_watch_point(donVi, cauHinh.low_limit);
  ESP_ERROR_CHECK(pcnt_unit_enable(donVi));
  ESP_ERROR_CHECK(pcnt_unit_clear_count(donVi));
  ESP_ERROR_CHECK(pcnt_unit_start(donVi));
}

int docDem() {
  int dem = 0;
  pcnt_unit_get_count(donVi, &dem);
  return dem;
}

float docGoc() {
  return (docDem() - demGoc) * 360.0f / DEM_MOI_VONG;
}

// duty từ -100 đến 100 %; 0 thì ngắt cả hai chiều
void xuatDuty(float u) {
  digitalWrite(IN1_PIN, u > 0 ? HIGH : LOW);
  digitalWrite(IN2_PIN, u < 0 ? HIGH : LOW);
  ledcWrite(ENA_PIN, (uint32_t)(fabsf(u) * 1023 / 100));
}

// Quỹ đạo hình thang: tăng tốc đều tới vMax, giảm tốc đều để dừng đúng mucTieu
void buocQuyDao() {
  float conLai = mucTieu - datQD;
  // Vận tốc lớn nhất còn kịp dừng đúng đích; trừ aMax*DT/2 vì vận tốc chỉ đổi theo từng bước 10 ms (không trừ thì vọt qua đích)
  float vKipDung = fmaxf(0.0f, sqrtf(2 * aMax * fabsf(conLai)) - aMax * DT / 2);
  float vMuon = copysignf(fminf(vMax, vKipDung), conLai);
  vQD += constrain(vMuon - vQD, -aMax * DT, aMax * DT);
  datQD += vQD * DT;
  if (fabsf(mucTieu - datQD) < aMax * DT * DT && fabsf(vQD) <= aMax * DT) {   // đủ gần: đặt đúng đích, dừng
    datQD = mucTieu;
    vQD = 0;
  }
}

// Một bước PID vị trí; vanTocLoc là vận tốc đo đã lọc (độ/s)
float buocPID(float dat, float doDuoc) {
  float saiSo = dat - doDuoc;
  if (fabsf(saiSo) < VUNG_NGHI && vQD == 0 && datQD == mucTieu) {   // đã tới đích: nghỉ, không rung
    tichPhan = 0;
    return 0;
  }
  float u = Kp * saiSo + Ki * tichPhan + Kd * (vQD - vanTocLoc);    // D theo sai lệch vận tốc
  if (u != 0) u += copysignf(dutyBu, u);                            // bù vùng chết của động cơ
  bool baoHoa = (u >= 100 && saiSo > 0) || (u <= -100 && saiSo < 0);
  if (!baoHoa && fabsf(saiSo) < VUNG_TICH_PHAN) tichPhan += saiSo * DT;
  return constrain(u, -100.0f, 100.0f);
}

void xuLyLenh() {
  String s = Serial.readStringUntil('\n');
  s.trim();
  if (s.length() < 1) return;
  char c = tolower(s.charAt(0));
  float so = s.substring(1).toFloat();
  if (c == 'g') mucTieu = so;
  else if (c == 'r') mucTieu += so;
  else if (c == 'z') {
    demGoc = docDem();
    mucTieu = datQD = vQD = 0;
    viTriTruoc = vanTocLoc = tichPhan = 0;
  }
  else if (c == 'v' && so > 0) vMax = so;
  else if (c == 'a' && so > 0) aMax = so;
  else if (c == 'p') Kp = so;
  else if (c == 'i') { Ki = so; tichPhan = 0; }
  else if (c == 'd') Kd = so;
  else if (c == 'b') dutyBu = constrain(so, 0.0f, 60.0f);
  Serial.printf("# dich %.1f do, v %.0f, a %.0f, Kp %.2f, Ki %.2f, Kd %.3f, bu %.0f%%\n",
                mucTieu, vMax, aMax, Kp, Ki, Kd, dutyBu);
}

void setup() {
  Serial.begin(115200);
  pinMode(IN1_PIN, OUTPUT);
  pinMode(IN2_PIN, OUTPUT);
  ledcAttach(ENA_PIN, 1000, 10);
  caiDatPCNT();
  xuatDuty(0);
}

void loop() {
  static unsigned long tTiepTheo = micros();
  static int lanIn = 0;

  if (Serial.available()) xuLyLenh();
  if ((long)(micros() - tTiepTheo) < 0) return;
  tTiepTheo += (unsigned long)(DT * 1e6);

  float goc = docGoc();
  float vanToc = (goc - viTriTruoc) / DT;
  viTriTruoc = goc;
  vanTocLoc += LOC * (vanToc - vanTocLoc);

  buocQuyDao();
  duty = buocPID(datQD, goc);
  xuatDuty(duty);

  if (++lanIn >= 3) {                                            // in mỗi 30 ms cho Serial Plotter
    lanIn = 0;
    Serial.printf("dich:%.1f,quy_dao:%.1f,goc:%.1f,duty:%.1f\n", mucTieu, datQD, goc, duty);
  }
}

Chương trình nhận lệnh qua Serial Monitor (115200, chọn New Line):

Mở Serial Plotter để xem bốn đường: đích, quỹ đạo, góc đo và duty. Mỗi 30 ms chương trình in một dòng theo dạng dich:…,quy_dao:…,goc:…,duty:….

Một số điểm trong chương trình:

• D theo sai lệch vận tốc Kd * (vQD - vanTocLoc): khi động cơ chạy nhanh hơn quỹ đạo, D kéo duty xuống; khi đứng yên ở đích, cả hai vận tốc bằng 0 nên D không gây giật.
• Bù vùng chết u += copysignf(dutyBu, u): mọi duty khác 0 đều cộng thêm phần đủ để thắng ma sát, nên sai số nhỏ vẫn tạo được chuyển động.
• Vùng nghỉ 0,5° (khoảng 2 xung): đã tới đích thì ngắt động cơ. Không có vùng nghỉ, phần bù vùng chết làm động cơ giật qua lại quanh đích vì encoder chỉ đo được từng bước 0,27°.
• Tích phân chỉ cộng khi sai số dưới 10° và đầu ra chưa bão hòa, tránh tích lũy lớn trong lúc chạy đường dài.

6. So sánh ba cách điều khiển

Hình trên là mô phỏng: phần điều khiển chạy đúng thuật toán của chương trình. Động cơ dùng cùng mô hình với bài giữ tốc độ: tốc độ đuổi theo duty với hằng số thời gian 0,15 s, vùng chết 20 % duty. Số trên động cơ thật sẽ khác, nhưng ba kiểu đáp ứng giống những gì thường thấy:

• Chỉ P, không bù (đỏ): lúc đầu sai số 90°, duty gần tối đa, động cơ chạy rất nhanh rồi vọt qua đích. Khi quay lại, duty giảm theo sai số; tới lúc duty rơi vào vùng chết thì động cơ đứng yên dù vẫn lệch 10°.
• P và bù vùng chết, chưa có D (cam): hết lệch nhưng không có gì hãm, động cơ dao động quanh đích nhiều lần mới yên.
• PD, bù vùng chết và quỹ đạo (xanh): điểm đặt đi dần tới đích, D hãm theo vận tốc, động cơ bám quỹ đạo và dừng đúng đích.

7. Chỉnh thông số trên mạch thật

Làm theo thứ tự, mỗi lần đổi một thông số và xem Serial Plotter. Trước khi chạy, cho trục ra quay tự do hoặc gắn tải nhẹ, không gắn vào cơ cấu có giới hạn hành trình.

1. Kiểm tra chiều: gửi p 1, d 0, b 0, rồi g 30. Nếu góc đo chạy ngược chiều đích và tăng mãi, encoder đang ngược chiều với động cơ: đổi chỗ hai dây động cơ ở OUT1, OUT2 (hoặc đổi A với B).
2. Đo duty bù vùng chết: gửi b 0, p 0.2, d 0, v 20, rồi r 360. Quỹ đạo đi chậm, sai số lớn dần, duty tăng dần. Đọc duty ở thời điểm đường góc đo bắt đầu tăng: đó là ngưỡng vùng chết. Đặt b thấp hơn ngưỡng này 1–2 %.
3. Kp: đặt lại v 360, gửi g 0, g 90 nhiều lần, tăng p tới khi bắt đầu thấy vọt lố hoặc dao động, rồi lùi về khoảng một nửa đến hai phần ba giá trị đó.
4. Kd: tăng d từng bước nhỏ tới khi hết vọt lố. Kd quá lớn làm duty nhảy loạn và động cơ kêu, do vận tốc đo từ encoder có độ phân giải thấp.
5. Ki: thường để 0. Chỉ thêm một lượng nhỏ (0,5–2) khi có tải luôn kéo về một phía, ví dụ cánh tay chịu trọng lực, làm động cơ dừng lệch đích.
6. vMax, aMax: xem tốc độ không tải ghi trên động cơ (ví dụ 200 vòng/phút là 1200 độ/s). Đặt vMax khoảng một phần ba tốc độ đó để còn dư duty cho PID bám theo; aMax lớn quá thì động cơ giật lúc khởi động.

Cách chỉnh PID chi tiết hơn (theo Ziegler–Nichols, theo dạng đáp ứng) nằm ở bài chỉnh thông số PID cùng cụm chủ đề.

8. Lỗi hay gặp

9. Xem thêm

• Giữ tốc độ động cơ DC ổn định bằng PID trên ESP32
• Đo tốc độ động cơ bằng encoder và bộ đếm xung PCNT của ESP32
• Encoder tương đối: cấu tạo, tín hiệu A B Z
• ESP32 điều khiển động cơ DC với driver L298N

Bài cùng cụm chủ đề: TB6612FNG và DRV8833, driver thay L298N cho xe robot; chỉnh thông số PID theo Ziegler–Nichols và chỉnh tay theo đáp ứng.