BÀI 51 – ROBOT 2 BÁNH ĐI THẲNG CHÍNH XÁC BẰNG 2 ENCODER + PID TRÊN STM32F103
Bài 50 chúng ta đã có 2 motor độc lập + 2 encoder + 2 PID tốc độ.
Bài 51 nâng cấp hệ thống thành:
Ra lệnh cho robot đi một quãng đường cụ thể, đồng thời tự sửa khi bánh trái/phải chạy không đều.
Đây là bước rất quan trọng để tiến tới robot tự hành.
1. Mục tiêu của Bài 51
Ví dụ ta muốn:
Robot → đi thẳng 100 cm → dừngKhông còn điều khiển kiểu:
PWM = 60%
delay(5000)
STOPmà sẽ là:
TARGET DISTANCE
↓
ENCODER POSITION
↓
POSITION CONTROL
↓
TARGET SPEED
↓
2 × SPEED PID
↓
2 × PWM
↓
2 MOTORĐồng thời:
Encoder Left
↕
Synchronization
↕
Encoder Rightđể robot cố gắng giữ đường thẳng.
2. Tại sao chỉ dùng 2 PID tốc độ vẫn chưa đủ?
Giả sử:
Target L = 100 RPM
Target R = 100 RPMHai PID làm rất tốt:
RPM L ≈ 100
RPM R ≈ 100Nhưng sau 5 giây có thể xảy ra:
Left = 5000 counts
Right = 4900 countsTại sao?
Encoder có thể có:
- sai số đo;
- bánh trượt;
- đường kính bánh khác nhau;
- tải khác nhau;
- thời gian đáp ứng khác nhau.
Vì vậy:
Cùng RPM không đảm bảo hai bánh đi được cùng quãng đường trong thực tế.
3. Ta cần thêm Position Control
Bây giờ hệ thống có hai tầng:
TARGET DISTANCE
↓
POSITION CONTROL
↓
TARGET SPEED
↓
┌───────────┴───────────┐
↓ ↓
SPEED PID L SPEED PID R
↓ ↓
PWM L PWM R
↓ ↓
MOTOR L MOTOR R
↓ ↓
ENCODER L ENCODER RĐây chính là kiến trúc cascade control mà chúng ta bắt đầu xây dựng ở Bài 49–50.
4. Đầu tiên phải hiểu counts
Encoder không đo:
cmmà đo:
countsVí dụ:
Encoder = 400 counts/revolutionBánh xe:
Diameter = 65 mmChu vi:
[
C=\pi D
]
[
C=3.1416\times65
]
≈
204.2 mmMỗi vòng bánh đi khoảng:
204.2 mm5. Tính counts/mm
Nếu:
400 countscho một vòng:
204.2 mmthì:
[
counts/mm =
\frac{400}{204.2}
]
≈
1.96 counts/mm6. Từ mm → encoder counts
Muốn robot đi:
1000 mmthì:
[
TargetCounts =
1000\times1.96
]
≈
1960 countsDo đó:
Robot_MoveDistance(1000);có thể chuyển thành:
target_counts = 1960;7. Nhưng có một vấn đề
Thông số trên chỉ là lý thuyết.
Đường kính bánh ghi:
65 mmkhông có nghĩa đường kính lăn thực tế chính xác 65 mm.
Có thể thực tế:
63.8 mmhoặc:
64.5 mmNgoài ra encoder có thể không phải đúng:
400 counts/revtheo cách mà chúng ta đang đếm.
Vì vậy robot cần calibration.
8. Calibration thực tế
Cho robot chạy:
1000 countsĐo khoảng cách thật.
Ví dụ:
Encoder = 1000
Actual distance = 510 mmthì:
[
counts/mm =
\frac{1000}{510}
]
≈
1.961Đây là thông số thực tế tốt hơn nhiều.
9. Tạo cấu hình
#define COUNTS_PER_MM 1.961fSau đó:
int32_t Distance_To_Counts(float mm)
{
return (int32_t)
(mm * COUNTS_PER_MM);
}Ví dụ:
Distance_To_Counts(1000.0f);→ khoảng:
1961 counts10. Cấu trúc Robot
Ta tạo:
typedef struct
{
int32_t left_start;
int32_t right_start;
int32_t target_counts;
float target_speed;
uint8_t moving;
} RobotMove_t;Khai báo:
RobotMove_t robot_move;11. Khi bắt đầu di chuyển
Ví dụ:
Robot_MoveDistance(1000.0f);ta lưu vị trí hiện tại:
robot_move.left_start =
left_encoder.position;
robot_move.right_start =
right_encoder.position;Sau đó:
robot_move.target_counts =
Distance_To_Counts(1000.0f);12. Tại sao phải lưu vị trí bắt đầu?
Giả sử trước khi ra lệnh:
Left position = 5000
Right position = 4900Ta muốn đi thêm:
1000 mmKhông thể yêu cầu:
Position = 1961vì đó là tọa độ tuyệt đối.
Ta cần:
Start + DistanceVí dụ:
Left target
= 5000 + 1961
= 696113. Tính quãng đường đã đi
Sau khi chạy:
left_delta =
left_encoder.position
- robot_move.left_start;
right_delta =
right_encoder.position
- robot_move.right_start;Ta có:
left_delta
right_delta14. Không nên chỉ dùng bánh trái
Sai lầm:
distance = left_delta;Nếu bánh trái trượt thì robot sẽ tính sai.
Ta có thể lấy trung bình:
[
distance =
\frac{left+right}{2}
]
Trong code:
float average_counts =
((float)left_delta +
(float)right_delta) / 2.0f;15. Đây là vị trí trung bình của robot
Robot
┌──────────────┐
│ │
O O
LEFT RIGHT
│ │
↓ ↓
1000 cnt 980 cnt
↓
Average =
990 cntTa dùng:
Average Encoderđể quyết định robot đã đi được bao xa.
16. Position Error
Giả sử:
Target = 2000 counts
Actual = 1000 countsthì:
error =
target_counts -
average_counts;=
1000 counts17. Position Controller
Giống Bài 49:
[
V_{target}=K_{pos}\times Error
]
Ví dụ:
#define POSITION_KP 0.05fNếu:
Error = 1000thì:
Target speed = 50 RPM18. Giới hạn tốc độ
#define MAX_MOVE_SPEED 100.0fSau đó:
if (target_speed >
MAX_MOVE_SPEED)
{
target_speed =
MAX_MOVE_SPEED;
}và:
if (target_speed <
-MAX_MOVE_SPEED)
{
target_speed =
-MAX_MOVE_SPEED;
}19. Nhưng robot vẫn có thể lệch
Ví dụ:
Left = 1000 counts
Right = 900 countsRobot đang có xu hướng lệch.
Ta cần:
Synchronization Controller20. Tính sai lệch hai bánh
float sync_error =
(float)left_delta -
(float)right_delta;Ví dụ:
Left = 1000
Right = 900thì:
sync_error = +10021. Tạo Correction
#define SYNC_KP 0.02fTa tính:
float correction =
SYNC_KP * sync_error;Ví dụ:
100 × 0.02=
2 RPM22. Điều chỉnh tốc độ
Nếu bánh trái đi xa hơn:
Left = 1000
Right = 900ta muốn:
Left chậm lại
Right nhanh lênDo đó:
target_left_rpm =
base_speed - correction;
target_right_rpm =
base_speed + correction;Kết quả:
Left = 98 RPM
Right = 102 RPM23. Đây là phần cực kỳ quan trọng
Hệ thống hiện tại có 3 tầng logic:
TARGET DISTANCE
│
▼
POSITION CONTROLLER
│
▼
BASE SPEED
│
▼
SYNC CONTROLLER
┌──┴──┐
▼ ▼
SPEED L SPEED R
│ │
▼ ▼
PID L PID R
│ │
▼ ▼
PWM L PWM RĐây là kiến trúc rất gần với robot thực tế.
24. Tuy nhiên có một vấn đề với SYNC_KP
Nếu correction quá lớn:
Base speed = 50
Correction = 80ta có:
Left = -30
Right = 130Robot có thể quay rất mạnh.
Vì vậy phải giới hạn correction:
#define MAX_SYNC_CORRECTION 20.0f25. Clamp correction
if (correction >
MAX_SYNC_CORRECTION)
{
correction =
MAX_SYNC_CORRECTION;
}
if (correction <
-MAX_SYNC_CORRECTION)
{
correction =
-MAX_SYNC_CORRECTION;
}26. Tạo hàm Clamp
Để code sạch hơn:
float Clamp(
float value,
float min,
float max)
{
if (value < min)
return min;
if (value > max)
return max;
return value;
}Sau đó:
correction =
Clamp(
correction,
-MAX_SYNC_CORRECTION,
MAX_SYNC_CORRECTION);27. Hàm Robot_MoveDistance()
Ta có thể viết:
void Robot_MoveDistance(
float distance_mm)
{
robot_move.left_start =
left_encoder.position;
robot_move.right_start =
right_encoder.position;
robot_move.target_counts =
Distance_To_Counts(
distance_mm);
robot_move.moving = 1;
}Ví dụ:
Robot_MoveDistance(1000.0f);→ robot đi khoảng 1 mét.
28. Hàm kiểm tra hoàn thành
uint8_t Robot_MoveFinished(void)
{
int32_t left_delta;
int32_t right_delta;
float average;
left_delta =
left_encoder.position -
robot_move.left_start;
right_delta =
right_encoder.position -
robot_move.right_start;
average =
((float)left_delta +
(float)right_delta)
/ 2.0f;
if (average >=
robot_move.target_counts)
{
return 1;
}
return 0;
}29. Khi đến đích
Không nên ngay lập tức:
PWM = 0;vì robot đang chạy.
Ví dụ:
100 RPM
↓
STOP PWM
↓
quán tính
↓
robot vẫn chạy
↓
vượt đíchDo đó position controller phải giảm tốc trước khi tới đích.
Đây chính là lý do chúng ta dùng:
Target Speed = Kp × Position Error30. Ví dụ giảm tốc
Giả sử:
Target = 2000Còn 1000 counts
Speed = 50 RPMCòn 500 counts
Speed = 25 RPMCòn 100 counts
Speed = 5 RPMCòn 20 counts
Speed = 1 RPMRobot sẽ tiến tới đích nhẹ nhàng hơn.
31. Vùng dừng
Ta tạo:
#define POSITION_TOLERANCE 5Nếu:
abs(error) <= 5thì:
target_left_rpm = 0;
target_right_rpm = 0;và:
robot_move.moving = 0;32. Nhưng với robot thực tế nên có STOP_ZONE
Ví dụ:
#define STOP_ZONE_COUNTS 5và:
#define SLOW_ZONE_COUNTS 300Khi còn:
> 300 counts→ chạy nhanh.
Khi:
< 300 counts→ giảm tốc.
Khi:
< 5 counts→ dừng.
33. State Machine
Bài này nên dùng:
typedef enum
{
ROBOT_IDLE,
ROBOT_MOVING,
ROBOT_SLOWING,
ROBOT_REACHED
} RobotState_t;34. ROBOT_IDLE
Không chạytarget_left_rpm = 0;
target_right_rpm = 0;35. ROBOT_MOVING
Error lớnRobot chạy về phía target.
36. ROBOT_SLOWING
Khi:
abs(error) < SLOW_ZONEPosition controller tự giảm tốc.
37. ROBOT_REACHED
Khi:
abs(error) <= TOLERANCEthì:
Speed target = 038. Code State Machine
void Robot_PositionControl(void)
{
float left_delta;
float right_delta;
float average;
float error;
float base_speed;
float sync_error;
float correction;
left_delta =
left_encoder.position -
robot_move.left_start;
right_delta =
right_encoder.position -
robot_move.right_start;
average =
(left_delta +
right_delta) / 2.0f;
error =
robot_move.target_counts -
average;
if (error < 0)
{
error = 0;
}
if (error <= POSITION_TOLERANCE)
{
robot_move.moving = 0;
target_left_rpm = 0;
target_right_rpm = 0;
return;
}
base_speed =
POSITION_KP * error;
base_speed =
Clamp(
base_speed,
0,
MAX_MOVE_SPEED);
sync_error =
left_delta -
right_delta;
correction =
SYNC_KP * sync_error;
correction =
Clamp(
correction,
-MAX_SYNC_CORRECTION,
MAX_SYNC_CORRECTION);
target_left_rpm =
base_speed -
correction;
target_right_rpm =
base_speed +
correction;
}Đây là phiên bản đầu tiên để học, sau này ta sẽ cải tiến nhiều.
39. Một vấn đề với code trên
Đoạn:
if (error < 0)
error = 0;chỉ phù hợp với:
Robot đi về phía trướcNếu muốn:
Robot_MoveDistance(-1000);để đi lùi, phải xử lý dấu.
Đây là một bài học quan trọng:
Không được tùy tiện biến error âm thành 0 nếu hệ thống cần chạy hai chiều.
40. Phiên bản hỗ trợ tiến/lùi
Ta lưu:
int32_t target_counts;có thể âm hoặc dương.
Ví dụ:
+1960 → tiến
-1960 → lùiSau đó:
error =
target_counts -
average;Ta không ép:
if(error < 0)
error = 0;mà xử lý dấu.
41. Tính base speed
base_speed =
POSITION_KP * error;Ví dụ:
error = -500→
base_speed = -25 RPMRobot tự lùi.
42. Nhưng synchronization cũng phải xét chiều
Nếu robot đang lùi:
Left = -100
Right = -90thì cần tránh viết correction một cách máy móc.
Ở phiên bản nâng cao, ta sẽ dùng signed traveled distance và quy ước encoder thật chặt chẽ.
Đây là lý do Bài 51 trước hết nên test đi thẳng tiến, sau đó mới mở rộng sang lùi.
43. Chu kỳ điều khiển
Ta vẫn sử dụng Timer từ các bài trước:
Timer tick = 1 msVí dụ:
1 ms
↓
system_tick++
10 ms
↓
Encoder_Update()
↓
Calculate_RPM()
↓
Speed_PID()
20 ms
↓
Position_Control()44. Tại sao Position Control có thể chậm hơn Speed PID?
Vì:
Speedthay đổi nhanh.
Trong khi:
Positionthay đổi tương đối chậm.
Ví dụ:
Speed PID = 100 Hz
Position loop = 50 Hztức:
10 ms
20 ms45. Luồng hoàn chỉnh
TARGET
1000 mm
│
▼
mm → Encoder
│
▼
Position Controller
│
Base Speed
│
┌──────┴──────┐
│ │
▼ ▼
Sync Control Sync Control
│ │
▼ ▼
Target L Target R
│ │
▼ ▼
PID LEFT PID RIGHT
│ │
▼ ▼
PWM LEFT PWM RIGHT
│ │
▼ ▼
MOTOR LEFT MOTOR RIGHT
│ │
▼ ▼
ENC LEFT ENC RIGHT
│ │
└──────┬──────┘
▼
Position46. Ví dụ thực tế
Giả sử:
Wheel diameter = 65 mm
Encoder = 400 counts/revRobot nhận:
Robot_MoveDistance(1000.0f);Mục tiêu:
1000 mmBan đầu:
Left = 0
Right = 0Position error:
1960 countsPosition controller:
Base speed = 100 RPMSau một thời gian:
Left = 800
Right = 760Average:
780Error:
1180Base speed:
59 RPMSync error:
40Correction:
0.8 RPMTarget:
Left = 58.2 RPM
Right = 59.8 RPMBánh phải được tăng nhẹ để bắt kịp.
47. Gần đích
Left = 1850
Right = 1830Average:
1840Error:
120Base:
6 RPMSync correction:
0.4 RPMTarget:
Left = 5.6 RPM
Right = 6.4 RPMRobot đi rất chậm.
48. Đích
Left = 1958
Right = 1959Average:
1958.5Nếu tolerance:
5 counts→ đạt.
Target RPM L = 0
Target RPM R = 0→ dừng.
49. Điều rất quan trọng: robot không nhất thiết phải có hai encoder giống hệt nhau
Ví dụ:
Left encoder:
400 CPR
Right encoder:
500 CPRvẫn có thể sử dụng.
Chỉ cần:
LEFT_COUNTS_PER_REV
RIGHT_COUNTS_PER_REVđược cấu hình riêng.
50. Đường kính hai bánh cũng có thể khác
Ta có:
#define LEFT_WHEEL_DIAMETER_MM 65.0f
#define RIGHT_WHEEL_DIAMETER_MM 64.5fKhi đó:
LEFT COUNTS/MM
RIGHT COUNTS/MMkhác nhau.
Đây chính là lý do calibration từng bánh rất hữu ích.
51. Calibration chuyên nghiệp hơn
Thay vì:
COUNTS_PER_MMmột giá trị chung, ta dùng:
float left_counts_per_mm;
float right_counts_per_mm;Khi robot đi:
1000 mmta có:
Left target counts
Right target countskhác nhau nếu cần.
52. Nhưng ở Bài 51 hãy bắt đầu đơn giản
Trình tự học nên là:
1. Hai encoder đọc đúng
2. Hai RPM đúng
3. Hai PID tốc độ ổn định
4. Robot chạy cùng RPM
5. Position control
6. Synchronization
7. Calibration
8. Đi thẳng chính xácĐừng đưa tất cả vào cùng một lúc.
53. Những lỗi thường gặp
Lỗi 1 – Robot quay thay vì đi thẳng
Kiểm tra:
Motor direction
Encoder directionLỗi 2 – PID tăng PWM liên tục
Có thể:
Encoder direction saiLỗi 3 – Robot đến gần đích rồi dao động
Giảm:
POSITION_KPhoặc giảm:
MAX_MOVE_SPEEDLỗi 4 – Robot luôn lệch một phía
Kiểm tra:
Left/Right encodervà:
SYNC_KPLỗi 5 – Robot vượt đích
Có thể:
MAX_MOVE_SPEED quá caohoặc:
POSITION_KP quá lớnhoặc PID tốc độ chưa được tune tốt.
54. Bài thực hành số 1
Cho:
Target = 500 mmRobot phải:
Đi 500 mm
↓
DừngĐo:
Actual distanceVí dụ:
Target = 500 mm
Actual = 527 mmSai số:
+27 mm55. Bài thực hành số 2
Đi:
1000 mmghi:
Left counts
Right countsVí dụ:
Left = 1960
Right = 1910Ta thấy:
Difference = 50 counts→ synchronization chưa đủ tốt hoặc có sai lệch cơ khí.
56. Bài thực hành số 3
Cho robot:
1000 mmvà:
SYNC_KP = 0Ghi lại độ lệch.
Sau đó:
SYNC_KP = 0.01rồi:
SYNC_KP = 0.02So sánh.
Bạn sẽ thực sự nhìn thấy tác dụng của feedback control.
57. Một khái niệm rất quan trọng từ Bài 51
Bạn đang bắt đầu chuyển từ:
"Điều khiển motor"sang:
"Điều khiển chuyển động của robot"Hai khái niệm này hoàn toàn khác nhau.
58. Bài 50
Ta nói:
Motor trái chạy 100 RPM
Motor phải chạy 100 RPMĐó là:
Motor control
59. Bài 51
Ta nói:
Robot đi 1000 mmĐó là:
Motion control
Đây là một bước tiến rất lớn.
60. Kiến trúc cuối của Bài 51
STM32F103
│
▼
TARGET DISTANCE
1000 mm
│
▼
POSITION CONTROLLER
│
BASE SPEED
│
▼
SYNC / STRAIGHT CONTROL
┌──────┴──────┐
▼ ▼
TARGET L TARGET R
│ │
▼ ▼
PID LEFT PID RIGHT
│ │
▼ ▼
PWM LEFT PWM RIGHT
│ │
▼ ▼
MOTOR LEFT MOTOR RIGHT
│ │
▼ ▼
ENCODER L ENCODER R
│ │
└──────┬──────┘
│
▼
ROBOT POSITIONĐây là nền tảng rất quan trọng cho robot 2 bánh.
61. Và từ đây Bài 52 sẽ còn thú vị hơn
Sau khi robot có thể:
đi 100 mm
đi 500 mm
đi 1000 mmvà tự giữ hai bánh tương đối đồng tốc, bước tiếp theo hợp lý là:
BÀI 52 – ĐIỀU KHIỂN GÓC QUAY ROBOT BẰNG 2 ENCODER
Chúng ta sẽ học cách ra lệnh:
Đi thẳng 500 mm
↓
Quay trái 90°
↓
Đi thẳng 1000 mm
↓
Quay phải 90°và từ đó bắt đầu xây dựng khái niệm:
DISTANCE
+
ANGLE
+
ENCODER
+
PID
↓
ODOMETRYĐó chính là bước đầu tiên để STM32 biết robot đang ở vị trí (X, Y) và góc hướng θ trên mặt phẳng.