Giữ tốc độ động cơ DC ổn định bằng PID trên ESP32: encoder, chỉnh Kp Ki Kd, Serial Plotter

Hướng dẫn điều khiển vòng kín tốc độ động cơ DC bằng PID trên ESP32: đo tốc độ bằng encoder và PCNT, chu kỳ 20 ms, chống tích phân bão hòa, chỉnh Kp Ki Kd và thí nghiệm STEM.

Đặt cùng một duty PWM, động cơ DC quay nhanh hay chậm còn tùy tải và điện áp pin: hãm nhẹ trục là tốc độ tụt, pin yếu đi là xe chạy chậm lại. Muốn giữ tốc độ đúng như mong muốn, ESP32 phải liên tục đo tốc độ thực bằng encoder, so với tốc độ đặt, rồi chỉnh duty. Cách chỉnh phổ biến nhất là PID.

Bài viết giải thích vòng điều khiển kín và ba thành phần P, I, D, chương trình PID chạy mỗi 20 ms trên ESP32, mô phỏng ảnh hưởng của các hệ số, cách chỉnh hệ số từng bước bằng Serial Plotter và gợi ý thí nghiệm STEM.

1. Vòng điều khiển kín và PID

• Sai số = tốc độ đặt − tốc độ đo được.
• P (tỉ lệ): duty tăng theo sai số. Chỉ dùng P thì thường còn sai số: động cơ cần một mức duty nhất định để quay, mà khi sai số nhỏ thì phần P cũng nhỏ.
• I (tích phân): cộng dồn sai số theo thời gian. Còn sai lệch thì phần I còn tăng, nên cuối cùng xóa được sai số còn lại.
• D (vi phân): phản ứng với việc tốc độ đang thay đổi nhanh, giúp giảm vọt lố. Đạo hàm khuếch đại nhiễu đo, nên với encoder thường chỉ cần Kd nhỏ hoặc bằng 0.

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

Dùng nguyên phần cứng của bài đo tốc độ động cơ bằng encoder trong cùng loạt:

Linh kiện: ESP32 DevKit V1, động cơ DC giảm tốc có encoder, module L298N, khay pin 18650 loại 2S.

3. Chương trình PID giữ tốc độ

Sửa DEM_MOI_VONG theo động cơ của bạn: số xung mỗi vòng × 4 × tỉ số hộp số. Lệnh trên Serial Monitor (115200 baud, "New Line"): s 120 đặt 120 vòng/phút (số âm thì quay ngược, s 0 dừng); p 0.3, i 1.5, d 0 đổi hệ số.

/*
  PYWORLD.VN - Giữ tốc độ động cơ DC ổn định bằng PID trên ESP32 (encoder đếm bằng PCNT, công suất qua L298N)
  Nối dây như bài đo tốc độ bằng encoder: A -> GPIO32, B -> GPIO33; ENA GPIO27, IN1 GPIO26, IN2 GPIO25
  Lệnh (115200, New Line): s 120 = đặt 120 vòng/phút (âm là quay ngược), p 0.3, i 1.5, d 0 = đổi hệ số; mở Serial Plotter để xem đồ thị
*/

#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.02f;                    // chu kỳ điều khiển 20 ms
const float LOC = 0.3f;                    // hệ số lọc tốc độ đo (0..1, nhỏ thì mượt hơn)

float Kp = 0.3f, Ki = 1.5f, Kd = 0.0f;     // đơn vị: % duty trên vòng/phút
float tocDoDat = 0;                         // vòng/phút
float tichPhan = 0, tocDoLoc = 0, tocDoLocTruoc = 0, duty = 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;
  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;
}

// duty từ -100 đến 100 %
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));
}

// Một bước PID: tính duty mới từ tốc độ đặt và tốc độ đo (đã lọc)
float buocPID(float dat, float doDuoc) {
  float saiSo = dat - doDuoc;
  float vi = -(doDuoc - tocDoLocTruoc) / DT;          // vi phân theo số đo, không theo sai số
  float u = Kp * saiSo + Ki * tichPhan + Kd * vi;
  // Chống tích phân bão hòa: chỉ cộng tích phân khi đầu ra chưa chạm giới hạn theo cùng chiều sai số
  bool bao_hoa = (u >= 100 && saiSo > 0) || (u <= -100 && saiSo < 0);
  if (!bao_hoa) 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 == 's') { tocDoDat = so; if (so == 0) tichPhan = 0; }
  else if (c == 'p') Kp = so;
  else if (c == 'i') { Ki = so; tichPhan = 0; }
  else if (c == 'd') Kd = so;
  Serial.printf("# dat %.0f vong/phut, Kp %.3f, Ki %.3f, Kd %.4f\n", tocDoDat, Kp, Ki, Kd);
}

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 demTruoc = 0;
  static int lanIn = 0;

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

  int dem = docDem();
  float tocDo = (dem - demTruoc) / DEM_MOI_VONG * 60.0f / DT;   // vòng/phút
  demTruoc = dem;
  tocDoLocTruoc = tocDoLoc;
  tocDoLoc += LOC * (tocDo - tocDoLoc);                         // lọc thông thấp bậc nhất

  duty = (tocDoDat == 0) ? 0 : buocPID(tocDoDat, tocDoLoc);
  xuatDuty(duty);

  if (++lanIn >= 3) {                                           // in 1 lần mỗi 60 ms cho Serial Plotter
    lanIn = 0;
    Serial.printf("dat:%.1f,do:%.1f,duty:%.1f\n", tocDoDat, tocDoLoc, duty);
  }
}

• Chu kỳ cố định 20 ms: biến tTiepTheo cộng dồn thời điểm chạy kế tiếp, nên các lần tính PID cách đều nhau. Đây là điều kiện để tích phân và vi phân có ý nghĩa.
• Lọc tốc độ đo: trong 20 ms, encoder 1320 đơn vị đếm mỗi vòng chỉ cho vài chục đơn vị đếm, nên tốc độ đo bị răng cưa. Bộ lọc tocDoLoc += 0,3 × (tocDo − tocDoLoc) làm mượt số đo.
• Vi phân theo số đo: lấy đạo hàm của tốc độ đo thay cho đạo hàm của sai số. Khi đổi tốc độ đặt đột ngột, duty không bị giật mạnh.
• Chống tích phân bão hòa: khi duty đã chạm 100 % mà vẫn thiếu, chương trình ngừng cộng tích phân. Nếu không, phần I phình to và tốc độ vọt lố rất lâu sau đó.
• Serial Plotter: dòng in dạng dat:120.0,do:118.4,duty:55.2 được Tools > Serial Plotter của Arduino IDE vẽ thành ba đường.

4. Kp, Ki ảnh hưởng thế nào

Mô phỏng dùng một động cơ có vùng chết 20 % duty. Kết quả cho thấy ba điều:

• Chỉ dùng P thì tốc độ dừng rất xa giá trị đặt. Thêm tải, tốc độ gần như về 0.
• PI với hệ số mặc định bám đúng 120 vòng/phút. Khi thêm tải, tốc độ tụt một chút rồi phần I đẩy duty lên để bù lại.
• Ki quá lớn thì tốc độ vọt lố và dao động không dứt.

Động cơ thật sẽ cho số khác, nhưng các xu hướng trên thường giống nhau.

5. Chỉnh hệ số từng bước

Hệ số tìm được chỉ đúng cho bộ động cơ, bánh xe và nguồn đang dùng. Đổi động cơ hoặc đổi số xung mỗi vòng thì phải chỉnh lại.

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

Mục tiêu: học sinh hiểu khái niệm phản hồi (feedback), thấy được sự khác nhau giữa điều khiển vòng hở và vòng kín, đọc đồ thị đáp ứng theo thời gian.

1. Vòng hở và vòng kín. Dùng chương trình bài encoder (vòng hở) đặt duty 60 % rồi hãm trục, ghi tốc độ. Dùng chương trình PID đặt đúng tốc độ đó rồi hãm trục với cùng lực. So sánh hai trường hợp.
2. Thi chỉnh PID. Mỗi nhóm tự chỉnh Kp, Ki, Kd. Tiêu chí: tốc độ lên tới 120 vòng/phút nhanh nhất mà vọt lố không quá 10 %. Chụp màn hình Serial Plotter để báo cáo.
3. Tìm ví dụ vòng kín quanh ta. Bàn luận các ví dụ như điều hòa giữ nhiệt độ, người lái xe giữ tốc độ, cơ thể giữ thân nhiệt. Trong mỗi ví dụ, đâu là cảm biến, đâu là bộ điều khiển, đâu là cơ cấu chấp hành?

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

8. Xem thêm

• PWM trên ESP32
• Ngắt GPIO trên ESP32 và đếm xung

Cùng loạt: đo tốc độ động cơ bằng encoder và PCNT, ESP32 điều khiển động cơ DC với L298N, xe robot ESP32 điều khiển qua Wi-Fi.