Bài đăng kỹ thuật
Triển khai và phân tích hệ thống
Vòng điều khiển tốc độ kín cho động cơ DC giảm tốc có encoder: STM32 đọc encoder bằng timer chế độ Encoder (x4), tính RPM mỗi 10 ms và giữ tốc độ đặt bằng bộ PI có feed-forward, kể cả khi tải thay đổi.
Ở dự án #15 chúng ta đã đo tốc độ động cơ bằng encoder. Dự án này đi tiếp một bước: dùng chính tốc độ đo được để tự động điều chỉnh PWM, giữ động cơ quay đúng tốc độ đặt khi điện áp nguồn hay tải thay đổi – nền tảng cho xe robot, băng tải mini và cơ cấu định vị.
1. Bài toán thực tế
Cùng một độ rộng xung PWM, động cơ sẽ quay chậm đi khi tải nặng hoặc pin yếu. Hai bánh xe robot dù cùng lệnh PWM vẫn quay khác tốc độ, khiến xe đi lệch. Cần vòng điều khiển kín: đo – so sánh – hiệu chỉnh liên tục.
2. Mục tiêu dự án
- Đọc encoder bằng phần cứng (timer Encoder Mode) để không mất xung khi tốc độ cao.
- Tính tốc độ trục ra (RPM) chu kỳ 10 ms, lọc nhiễu bậc nhất.
- Bộ điều khiển PI + feed-forward, chống bão hòa, đảo chiều mượt.
- Đánh giá chất lượng: thời gian đáp ứng, sai số xác lập, khả năng chịu tải.
3. Phần cứng sử dụng
| Thiết bị | Thông số chính | Vai trò |
|---|---|---|
| Động cơ DC giảm tốc có encoder (ví dụ JGB37-520 12 V) | Encoder Hall 2 kênh A/B, 11 xung/vòng trục động cơ, hộp số 1:30 | Đối tượng điều khiển |
| Driver TB6612FNG (≤ 1,2 A) hoặc BTS7960 (dòng lớn) | PWM tới 20 kHz+, chân chiều | Khuếch đại công suất |
| STM32F103C8T6 | TIM3 Encoder Mode, TIM1 PWM | Đo tốc độ, tính PI |
| Nguồn 12 V | Đủ dòng khởi động động cơ | Cấp cho driver |
Với encoder 11 xung/vòng và hộp số 1:30, trục ra có 11 × 30 = 330 xung/vòng mỗi kênh; đếm cả 4 sườn của A và B (x4) được 1320 số đếm/vòng.
4. Sơ đồ khối và đấu nối

Kênh A/B của encoder vào TIM3_CH1/CH2 (PA6/PA7). TIM3 tự tăng/giảm bộ đếm theo chiều quay – CPU không cần ngắt cho từng xung. TIM1_CH1 (PA8) phát PWM 20 kHz cho driver, hai chân GPIO chọn chiều. TIM2 tạo ngắt 100 Hz chạy vòng điều khiển.
Bảng đấu nối
| Tín hiệu | STM32 | Thiết bị |
|---|---|---|
| Encoder A / B | PA6 / PA7 (TIM3 CH1/CH2) | Encoder (nguồn 3,3–5 V, chân FT chịu 5 V) |
| PWM | PA8 (TIM1 CH1) | PWMA (TB6612) / RPWM-LPWM (BTS7960) |
| Chiều quay | PB12, PB13 | AIN1, AIN2 (TB6612) |
| STBY | PB14 (mức 1) | Cho phép driver |
5. Nguyên lý và thuật toán
Tính tốc độ. Mỗi 10 ms đọc bộ đếm TIM3, lấy hiệu so với lần trước (ép kiểu int16_t để tự xử lý tràn và chiều âm). RPM = Δcount × 60 / (1320 × 0,01). Ở tốc độ thấp số xung trong 10 ms ít nên RPM bị "bậc thang"; lọc thông thấp bậc nhất rpm_f += 0.3*(rpm - rpm_f) giúp mượt hơn.
Feed-forward + PI. Đo trước quan hệ gần đúng duty ≈ k·RPM (ở không tải). Phần feed-forward đưa ngay động cơ tới gần tốc độ đặt, phần PI chỉ phải bù sai lệch do tải – nhờ đó đáp ứng nhanh mà không cần Kp quá lớn. Không dùng khâu D vì tín hiệu tốc độ từ encoder có nhiễu lượng tử.
Anti-windup. Khi duty đã chạm ±100 %, ngừng cộng dồn tích phân để tránh vọt lố khi tải được giải phóng.
6. Chương trình mẫu
Ngắt TIM2 100 Hz: đo tốc độ và tính PI (TIM3 Encoder Mode TI1 and TI2, Period 65535; TIM1 PWM Period 3599 → 20 kHz):
#define CPR 1320.0f // số đếm/vòng trục ra (x4)
#define DT 0.01f
#define KFF 0.45f // %duty trên mỗi RPM (đo thực nghiệm)
float kp = 0.6f, ki = 6.0f;
volatile float rpmSet = 120.0f, rpmF = 0, integ = 0;
static void motor_out(float duty) // duty: -100..100 %
{
uint8_t fwd = duty >= 0;
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, fwd ? GPIO_PIN_SET : GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, fwd ? GPIO_PIN_RESET : GPIO_PIN_SET);
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1,
(uint32_t)(fabsf(duty) * 3599 / 100));
}
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if (htim->Instance != TIM2) return;
static uint16_t last = 0;
uint16_t now = __HAL_TIM_GET_COUNTER(&htim3);
int16_t delta = (int16_t)(now - last); // tự xử lý tràn và chiều quay
last = now;
float rpm = delta * 60.0f / (CPR * DT);
rpmF += 0.3f * (rpm - rpmF);
float e = rpmSet - rpmF;
float out = KFF * rpmSet + kp * e + integ;
if (out > -100.0f && out < 100.0f) integ += ki * e * DT; // anti-windup
if (out > 100.0f) out = 100.0f;
if (out < -100.0f) out = -100.0f;
motor_out(out);
}Nhớ gọi HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL), HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1) và HAL_TIM_Base_Start_IT(&htim2) trước vòng lặp chính.
7. Các bước triển khai và kiểm thử
- Quay tay trục động cơ đúng 1 vòng, kiểm tra bộ đếm TIM3 thay đổi ±1320 (đúng chiều).
- Chạy hở mạch (chỉ feed-forward), ghi bảng duty → RPM ở 20/40/60/80/100 % để tính hệ số KFF.
- Bật PI với Ki = 0, tăng Kp tới khi đáp ứng nhanh mà chưa dao động; sau đó tăng Ki để triệt sai số xác lập.
- Thử bước tốc độ 60 → 150 RPM, gửi rpmF qua UART và vẽ đồ thị bằng Serial Plotter.
- Dùng tay hãm nhẹ trục: tốc độ phải tự hồi phục về giá trị đặt trong vài trăm ms.
8. Lỗi thường gặp và cách xử lý
- Tốc độ âm khi quay thuận: đảo A/B hoặc đổi cực tính kênh trong cấu hình encoder.
- Động cơ rung, kêu rít: Kp quá lớn hoặc tần số PWM nằm trong dải nghe được – nâng PWM lên ~20 kHz.
- RPM nhảy mạnh ở tốc độ thấp: tăng chu kỳ đo (20–50 ms) hoặc đo chu kỳ xung bằng Input Capture.
- Driver nóng: TB6612 chỉ khoảng 1,2 A liên tục; động cơ lớn cần BTS7960 hoặc driver MOSFET.
9. Hướng mở rộng
- Ghép thêm vòng vị trí (cascade) để quay chính xác số vòng/góc.
- Hai động cơ cho xe vi sai, nhận lệnh vận tốc tuyến tính/góc từ ROS (xem dự án #7, #42).
- Tự động chỉnh tham số (auto-tune) bằng phương pháp relay feedback.