Robot bám đường ứng dụng thuật toán pid line follow robot with pid (chu quang thảo)

Mục Lục
CHƯƠNG I: TỔNG QUAN VỀ ROBOT ..........................................................................................3
I.1 Tổng quan về Robot..................................................................................................................3
I.2.1 Giới thiệu về Robot bám đường tự động............................................................................3
I.2.2 Nguyên tắc hoạt động chung của Robot bám đường tự động.............................................4
I.2.3 Ứng dụng của robot bám đường tự động...........................................................................4
HƯƠNG II: THIẾT KẾ MƠ HÌNH ROBOT BÁM ĐƯỜNG TỰ ĐỘNG..........................................5
II.1 Cấu trúc cơ bản của robot dò đường tự động ..........................................................................5
II.2 Thiết kế cơ khí.........................................................................................................................6
II.2.1 Ý tưởng :..........................................................................................................................6
II.2.2 Khung xe :........................................................................................................................6
II.2.3 Lựa chọn phần tử và kết cấu, mô phỏng 3D ......................................................................6
II.2 Phần điều khiển..................................................................................................................... 10
II.2.1 Tổng quan và sơ đồ khối : .............................................................................................. 10
II.2.2 Các khối cơ bản của robot :............................................................................................ 11
CHƯƠNG III: XÂY DỰNG THUẬT TOÁN CHO ROBOT BÁM ĐƯỜNG. .................................. 16
III.1 GIÁI THUẬT ...................................................................................................................... 16
III.1 Ý tưởng giải thuật ............................................................................................................ 16
III.2 Lưu đồ thuật toán: ........................................................................................................... 20
III.2 CODE .................................................................................................................................. 22
CHƯƠNG IV: KẾT QUẢ THỰC NGHIỆM ................................................................................... 29
IV.1 Robot ................................................................................................................................... 29
IV.2 Kết quả chạy và đánh giá. .................................................................................................... 29
CHƯƠNG V: KẾT LUẬN VÀ KIẾN NGHỊ.................................................................................... 30
V.1 Kết luận:................................................................................................................................ 30
V.2 Kiến nghị:.............................................................................................................................. 30

1


LỜI NÓI ĐẦU

Cơ - Điện tử là ngành khoa học tổng hợp liên ngành của cơ khí chính xác,
điện tử, điều khiển và tư duy hệ thống trong thiết kế và phát triển sản phẩm. Đây là
ngành rất quan trọng và không thể thiếu trong sự phát triển của khoa học kỹ thuật
hiện đại.
Với bất kỳ khóa đào tạo nào, q trình thực tập ln có ý nghĩa rất quan trọng
đối với mỗi sinh viên. Thực tập tốt nghiệp giúp sinh viên có thể tổng hợp được các
kiến thức chung về cơ khí, điện tử, lập trình … đã được tích lũy từ các năm học
trước. Đồng thời có thể phát triển thêm, nhận thức thực tế hơn về sự tích hợp của
Cơ – Điện tử trong thực tế, có thể biến những lý thuyết trước đây để ứng dụng tạo
thành những sản phẩm thực. Khơng những thế, q trình thực tập còn giúp sinh
viên đúc kết những kinh nghiệm quý báu, những kỹ năng làm việc có kế hoạch, làm
việc theo nhóm, … Đó là những hành trang hết sức thiết thực cho mỗi sinh viên
trong tương lai sau này.
Thực tập tốt nghiệp tại trường đã giúp chúng tôi có cơ hội học tập và trau dồi
thêm kiến thức cịn thiếu. Chúng tơi xin chân thành cảm ơn sự giúp đỡ tận tình của
các thầy giáo hướng dẫn cùng với sự ủng hộ , tạo điều kiện của khoa và nhà trường.
Nhóm làm báo cáo:
Chu Quang Thảo
Trịnh Tài Tuấn
Phan Cao Khánh

2


CHƯƠNG I: TỔNG QUAN VỀ ROBOT
GIỚI THIỆU VỀ ROBOT BÁM ĐƯỜNG TỰ ĐỘNG
I.1 Tổng quan về Robot
Robot đã và đang xuất hiện trong cuộc sống của chúng ta từ lâu và ngày càng
trở thành một phần không thể thiếu trong cuộc sống hiện đại. Chúng đã góp phần
mình vào cơng cuộc lao động, chính robot đang làm nên một cuộc cách mạng về

lao động, khoa học, và đang phục vụ đắc lực cho các ngành khoa học như: Khoa
học quân sự, khoa học giáo dục, các ngành dịch vụ, giải trí…
Vậy robot xuất hiện từ khi nào? Năm 1921 nhà soạn kịch Karel Capek người
Tiệp Khắc đã đưa lên sân khấu vở kịch có tiêu đề “Romands Univesal Robot”.
Theo tiếng Séc “Robot” nghĩa là “Người tạp dịch”. Có thể nói đây là một gợi ý,
một ý tưởng ban đầu về những cỗ máy có khả năng thao tác như con người. Đến
trước chiến tranh thế giới lần thứ hai nhu cầu sử dụng những máy móc có khả năng
thay thế con người ở những môi trường làm việc độc hại đã trở thành một nhu cầu
cấp thiết. Ban đầu cơ cấu máy này hoạt động giống như tay máy của người vận
hành. Cấu tạo của cơ cấu này bao gồm các thanh và các khớp và hệ thống giây
chằng. Người vận hành điều khiển tay máy thông qua một cơ cấu khuyếch đại cơ
khí.Trong chiến tranh thế giới lần thứ hai (năm 1945), xuất hiện cơ cấu máy được
điều khiển từ xa để cầm nắm chất phóng xạ. Cho đến những năm 1950 cùng với sự
ra đời của kỹ thuật điều khiển chương trình số NC (Number Control) kỹ thuật tay
máy lúc này đã kết hợp được cả kỹ thuật điều khiển xa và điều khiển chương trình
số. Sự kết hợp này đã tạo ra những thế hệ máy điều khiển từ xa có khả năng mềm
dẻo, khả năng tự động hoá cao gọi tên là robot.
Năm 1949, máy phay điều khiển số ra đời phục vụ sản xuất ở Mỹ. Đến năm
1960 George Devol đưa ra mẫu Robot đầu tiên. Năm 1961 cũng tại Mỹ Robot công
nghiệp. (IR: Industrial Robot) đầu tiên đưa ra thị trường: Robot Unimat 1990 (Do
trường đại học MIT chế tạo) đây là Robot phản hồi lực nó được ứng dụng vào cơng
nghiệp sản xuất ô tô. Theo con số thống kê thì đến năm 1990 toàn thế giới đã triển
khai và ứng dụng khoảng 300.000 IR. Do sự phát triển mạnh mẽ của kỹ thuật Vi xử
lý và Tin học mà số lượng IR tăng nhanh chóng và tính năng cũng có nhiều bước
đột phá, giá thành trên một đơn vị IR giảm dần.
I.2.1 Giới thiệu về Robot bám đường tự động.
Loại robot mà việc di động theo một quĩ đạo định sẵn được gọi là Robot bám
đường. Robot bám đường có thể di chuyển theo một đường, đường đi có thể được

3

nhìn nhận như một dịng màu đen trên một bề mặt trắng (hoặc ngược lại) hoặc nó
có thể là đường vơ hình như một từ trường.
I.2.2 Ngun tắc hoạt động chung của Robot bám đường tự động.
Robot chuyển động theo một quỹ đạo được định trước nhờ vạch dẫn, hệ thống
hai bánh xe được dẫn động bởi 2 động cơ điện một chiều thông qua mạch điều
khiển và mạch công suất. Thường thì các vạch dẫn sẽ có màu khác với màu nền của
quỹ đạo chuyển động. Để Robot chuyển động đúng quỹ đạo cần có bộ phận cảm
biến, bộ phận này có nhiệm vụ phân biệt vạch dẫn và màu nền của quỹ đạo chuyển
động. Để Robot chuyển động đúng theo quỹ đạo cần có bộ phận cảm biến, bộ phần
này có nhiệm vụ phân biệt vạch dẫn và màu nền, đưa tín hiệu điện tương ứng về
mạch điều khiển. Mạch điều khiển có nhiệm vụ thu nhận thơng tin phản hồi từ bộ
phận cảm biến từ đó điều khiển tốc độ và chiều quay của hai động cơ điện một
chiều sao cho xe luôn bám và chuyển động theo vạch dẫn. Nhìn chung, về mặt cấu
tạo thì Robot tự động bám đường được thể hiện theo sơ đồ sau đây.

Hình 1: Cấu tạo chung của Robot bám đường tự động.
I.2.3 Ứng dụng của robot bám đường tự động.
Được ứng dụng vào cơng nghệ dị đường như di chuyển tự động trong vận
chuyển hàng hóa. Tuy cịn sơ khai về nguyên tắc điều khiển nhưng robot dò đường
4


đã tạo một bước ngoặt quan trọng trong công nghệ thiết kế và chế tạo robot, mở ra
một kỷ nguyên mới về ngành cơng nghệ kỹ thuật tự động hóa và đưa tự động hóa
vào sản xuất.
HƯƠNG II: THIẾT KẾ MƠ HÌNH ROBOT BÁM ĐƯỜNG TỰ ĐỘNG.
II.1 Cấu trúc cơ bản của robot dò đường tự động
KHỐI HIỂN THỊ VÀ

CẢM BIẾN
(Cảm biến dị đường)

KHỐI XỬ LÝ
TRUNG TÂM
(Atmega16)

KHỐI NGUỒN VÀ
CƠNG TẮC HÀNH
TRÌNH

MẠCH CƠNG SUẤT
ĐIỀU KHIỂN ĐỘNG
CƠ BÁNH TRÁI

ĐIỀU KHIỂN ĐỘNG
CƠ BÁNH PHẢI

M1

M2
BỘ PHẬN CHẤP HÀNH

Hình 2: Sơ đồ cấu trúc cơ bản của robot dị đường tự động
Trong đó:
- Khối hiển thị và cảm biến: là 8 led hiển thị cho cảm biến dò đường và
8 led thu hồng ngoại nhận tín hiệu vạch dẫn đường.
- Khối nguồn và cơng tắc hành trình: Các cơng tắc hành trình sử dụng
cho việc xác định trạng thái của cơ cấu chấp hành và robot. Nguồn
cung cấp cho khối xử lý trung tâm, khối mạch công suất điều chỉnh

cấp nguồn cho khối chấp hành.
- Khối xử lý trung tâm: quan trọng nhất là chip Atmega16 điều khiển
tồn bộ hoạt động của robot: nhận tín hiệu khối cảm biến. sau đó gửi
tín hiệu qua khối hiển thị và điều khiển khối cơ cấu chấp hành.
- Khối cơ cấu chấp hành : là mạch công suất dùng để điều khiển động
cơ bánh xe.
5


II.2 Thiết kế cơ khí
II.2.1 Ý tưởng :
Theo yêu cầu robot đơn thuần là bám đường tự đơng, ngồi ra không yêu cầu
các chức năng khác nên phần cơ không cần quá cầu kì, nên lựa chọn những vật liệu
vừa đảm bảo độ bền cho robot, vừa có giá thành phù hợp với sinh viên.
Có nhiều chi tiết gá trên robot, trong đó cần chú ý tới 3 mạch gồm 1 mạch
điều khiển,và 2 mạch cơng suất kích thước, độ cao linh kiện, dây nối giữa các linh
kiện, nên cần thiết kế không gian Robot đảm bảo cho việc gá các chi tiết của robot,
mạch, RC servo…
II.2.2 Khung xe :
Là nơi gá đặt các bộ phận của robot như hệ thống bánh xe, động cơ, mạch điều
khiển, mạch cảm biến,… Khung xe đơn giản có thể làm bằng các tấm phẳng bằng
gỗ, fip, mika, alumiun,…; Tùy theo hình dạng và vị trí gá đặt các thành phần trên
mà thiết kế khung xe cho hợp lý.
Cách bố trí trọng tâm.
Trọng tâm hiểu đơn giản là nơi phân bố đều sức nặng của Robot, vậy
trọng tâm có gì quan trọng. Trọng tâm đóng vai trị quan trọng trong việc
giữ cân bằng cho Robot lúc di chuyển , nhất là với những robot có mặt
chân đế nhỏ và có kết cấu cao,việc bảo đảm bảo vị trí trọng tâm một cách
hợp lý cịn giúp rất nhiều cho người lập trình trong viết chương trình dị
đường cho Robot.

Phần truyền lực chính của cả khung đế được quyết định bởi hai động cơ
chạy, khi khung đế xoay ( bẻ cua ) thì tâm xoay nằm tại trung điểm của
trục nối 2 bánh chủ động ( điểm màu đỏ ) như vậy lực xoay xe được quyết
định bởi khoảng cách d và trọng lượng của Robot như bạn biết thì Momem
xoay ( M ) ở mỗi bánh có thể thiết lập đơn giản là :
F = ( Momem quay của động cơ )/ (chu vi của bánh xe chủ động )
M = F x d/2 (khi chưa xét tới vị trí của trọng tâm và độ dài a của Robot)

(Có thể tạm xem ma sát giữa các bánh và sàn là như nhau)
Vậy thì theo cơng thức có thể hiểu nếu Robot có cùng trọng lượng và chiều
dài thì Robot có khoảng cách d càng lớn thì xoay xe càng dể.
II.2.3 Lựa chọn phần tử và kết cấu, mô phỏng 3D
Sử dụng phần mềm Autodesk Inventor để mô phỏng 3D robot dựa trên ý
tưởng định sẵn. Mơ hình 3D thiết kế trên Autodesk Inventor là một mơ hình số 3D
chính xác, cho phép người dùng kiểm sốt hình dạng, thuộc tính, và các chức năng
của mơt thiết kế, giới hạn bớt nhu cầu đối với các mơ hình vật lý, cũng như giảm
bớt chi phí thay đổi thiết kế như trong thiết kế truyền thống khi đưa ra sản xuất.
6


Lựa chọn phần tử và kết cấu:
 Giá đỡ chính kích thước 250x180mm có tác dụng là khung xe chính để liên
kết các chi tiết lại với nhau.
 Hai bánh xe có đường kính 60mm được gắn vào trục động cơ
 Một bánh xe đa hướng gắn ở đầu xe
 Động cơ điện một chiều kích từ động lập tỉ số vòng quay 110 vg /ph.
 Nguồn acquy 12V.
 Mạch CPU nhiệm vụ thu nhận ,phân tích và phát lệnh điều khiển.
 Mạch cảm biến thu nhận thông tin cung cấp cho mạch điều khiển.
 Mạch công suất khuếch đại tín hiệu điều khiển.

 Vít , dây nạp , dây nguồn …
Bảng 1: Lựa chọn chi tiết cơ khí cho robot.
Tên
Số
Thơng số
Hình ảnh
lượng
Khung xe
2
- Chất liệu:
mika.
- Kích thước
210x280x2,5
(mm).

Tấm gá động
cơ và khung

2

Động cơ

2

Khớp nối L

8

- Chất liệu:
mika.

- Kích thước
80x130x2,5
(mm).
- Tự chế tạo .
- Động cơ có
hộp giảm tốc.
- Mua.

- Chất liệu:
thép.
- Mua.

7


Bánh dẫn
hướng

1

- Mua.

Bánh xe

2

- Mua.

Chốt bánh xe

2

- Chất liệu:
thép.
- Mua.

Bulong 3x30

2

- Chất liệu:
thép.
- Mua.
- Gá sensor và
toàn bộ mạch

Bulong 3x10

22

- Chất liệu:
thép.
- Mua.
- Gá khung.

Bulong 5x25

2

- Chất liệu:

thép.
- Mua.
- Gá mắt trâu.

Đai ốc các
loại(2,3,5mm
)

30

- Chất liệu:
thép.
- Mua.
- Lắpvới
bulong.

22
6

8


Nguồn

1

- Mua.

Mơ hình 3D: Khung robot:

Hình 3: Khung 3D robot
Robot hồn thiện phần cơ khí:

Hình 4: Phần cơ khí hồn thiện.

9


II.2 Phần điều khiển
II.2.1 Tổng quan và sơ đồ khối :
Robot bám đường là loại robot di động có khả năng bám theo một vạch có màu
phân biệt với mơi trường.
Phạm vi ứng dụng của loại robot này chủ yếu là trong cơng việc vận chuyển
hàng hóa tự động trong nhà máy, cảng biển, quốc phịng an ninh, hàng khơng vũ trụ.
Xe dị đường trên thực tế có rất nhiều dạng. Loại đơn giản nhất mà sinh viên
thường biết đến là loại xe 2 bánh chủ động,1 bánh đa hướng và 1 mạch dị đường
như được cho ở hình sau:

Hình 5: Sơ đồ khối cơ bản của xe bám đường.

10


II.2.2 Các khối cơ bản của robot :
II.2.2.1 Khối nguồn cấp cho Robot :

Hình 6: Khối nguồn trên CPU.
Dùng nguồn 9-24V để cấp điện áp vào, sau đó thơng qua khối lọc, hạ áp dùng
LM2576 ta lấy điện áp 5V ổn định để cung cấp cho khối còn lại trên mạch điều
khiển.

Hình 7: Khối nguồn trên mạch cơng suất.
Trên mạch công suất, dùng nguồn 24V để cấp điện áp vào, sau đó thơng qua
khối lọc, hạ áp dùng LM2576 ta lấy điện áp 12V ổn định để cung cấp cho động cơ.
II.2.2.2 Khối cảm biến dò đường :
Cảm biến quang dị đường là thành phần quan trọng khơng thể thiếu trong
điều khiển lộ trình chạy của Robot một cách chính xác và đẹp mắt. Sơ đồ mạch
được lựa chọn là kiểu thông dụng do đơn giản nhưng để nâng cao khả năng nhận
dạng vạch trắng (hoặc vạch màu) và phù hợp với việc dùng chung nguồn vi điều
11


khiển +12V, ta sẽ nâng cao điện áp cung cấp cho cảm biến, tăng số led phát ánh
sáng trắng.
Trước hết, cần có quan niệm ánh sáng gồm ánh sáng nhìn thấy được (có màu
sắc) và ánh sáng khơng thấy được (hồng ngoại….). Và chúng ta đều biết, màu sắc
của một vật là do ánh sáng phản xạ từ nó ra, tức là một vật có màu nào thì sẽ hấp
thu tất cả ánh sáng khác trừ màu của chính nó. Trong đó, màu trắng là màu phản xạ
lại tất cả ánh sáng chiếu tới, còn màu đen là màu hấp thụ tồn bộ ánh sáng. Sân thi
đấu Robocon nhìn chung là một nền màu sẫm được dán vạch trắng thành các ơ
vng. Bộ dị đường gồm tập hợp một hàng led phát và một hàng cảm biến quang
(light sensor) như quang trở, IR sensor…. có khả năng thay đổi điện trở khi có ánh
sáng chiếu vào.Từ đó, nếu như cùng một thứ ánh sáng phát ra từ led phát của bộ dò
đường, vạch trắng sẽ phản xạ nhiều nhất, điện trở của cảm biến sẽ nhỏ nhất, ngược
lại nền màu sẫm sẽ hấp thu một phần ánh sáng, cảm biến sẽ có điện trở lớn hơn.
Giá trị điện áp trên cảm biến thu được đưa về bộ ADC qua một mạch đệm, từ đó
VĐK sẽ quyết định đó là vạch hay nền.

Hình 8: Sơ đồ nguyên lý 1 cặp led thu phát cảm biến dò đường.
a. Diode thu quang (photodiode)

Photodiode hay diode hồng ngoại thực chất là một mối nối bán dẫn P-N có đặc
tính thay đổi cường độ dịng điện qua khi có ánh sáng kích thích lớp tiếp giáp.
Trong mạch nó được mắc nối tiếp với một điện trở lớn và mắc phân cực ngược.Khi
bị che sáng, nó dẫn điện yếu, và dẫn mạnh hơn khi có ánh sáng chiếu vào.
Photodiode có tần số đáp ứng nhanh, phù hợp với bộ dò đường. Và trên thực tế
BKFET và rất nhiều đội khác đã dùng loại cảm biến này cho bộ dò đường. Để dễ
hiểu chúng ta sẽ thống nhất gọi cảm biến là LED thu.
b. Led phát
12


Loại ánh sáng của led phát có ảnh hưởng trực tiếp tới khả năng nhận dạng của
cảm biến.
Nếu sử dụng quang trở thì màu sắc của led phát khơng quan trọng vì nó chỉ phụ
thuộc vào cường độ sáng. Tuy nhiên trên thực tế chúng ta sử dụng diode hông
ngoại và cảm biến này chỉ phụ thuộc vào cường độ của hồng ngoại mà thôi.
Điện áp đặt trên các Led siêu sáng trắng có điện áp khoảng 2V, dịng qua Led từ
10 đến 15mA, Ta có thể chọn điện trở hạn dòng cho led siêu sáng:
𝑉 −𝑉
5−2
Rled = 𝑐𝑐 𝑙𝑒𝑑
=
=200Ω
−3
15.10
15.10 −3
Chọn Rled = 220 Ω
Ở đây ta dùng led siêu sáng và led thu hồng ngoại để cảm biến dò đường. Khi
led phát tín hiệu và có tín hiệu phản lại thì led thu hồng ngoại sẽ thay đổi điện trở
nó phát tín hiệu đưa vào vi điều khiển Atmega16 có sẵn bộ chuyển đổi ADC 10 bit

xử lý rồi đưa ra tín hiệu phát lệnh điều khiển động cơ.
 Thêm một vài chú ý:
- LED thu, phát có góc sáng rất hạn chế, vì thế cách bố trị chúng cũng rất quan
trọng. Khoảng cách giữa 2 LED nên từ 0.5 đến 1cm. Cần đặt song song và có
mối liên hệ tốt cũng như tách biệt với bộ khác. Khoảng cách giữa các bộ LED
lại phụ thuộc vào sân thi đấu từng năm cũng như giải thuật dò đường.
- Khoảng cách từ LED tới mặt nền cũng rất quan trọng. Khoảng cách cụ thể
yêu cầu xác định bằng thực nghiệm.
- Rất nhiều vật có khả năng che chắn ánh sáng màu nhưng lại khơng có khả
năng ngăn tia hồng ngoại. Vì thế trong thực tế cũng như thí nghiệm cấn che
chắn bộ dò đường cẩn thận.
II.2.2.3 Khối động lực:
Loại động cơ ta sử dụng là 2 động cơ DC – 12V , phục vụ cho việc điều khiển xe
tiến, lùi, tăng tốc ,quay trái, quay phải, với các cấp tốc độ khác nhau.

Hình 9: Sơ đồ mạch cấp nguồn điều khiển động cơ
13


Trong quá trình thực tập, điều khiển động cơ phải liên tục thay đổi tốc độ, đảo
chiều động cơ, hãm động cơ, cuộn dây động cơ sẽ sinh ra các suất điện động là các
xung dirac, các xung này có biên độ rất lớn, các xung này sẽ đánh lên mạch điều
khiển theo đường nguồn hay max, kết quả là VĐK sẽ hay bị treo, hoặc có thể hỏng.
Có 2 cách khắc phục là dùng opto còn gọi là bộ cách ly quang (opto-isolator), rất
hay được sử dụng trong các hệ thống điện-điện tử công suất lớn, dùng để ngăn các
xung điện áp cao hay các phần mạch điện công suất lớn có thể làm hư hỏng các ngõ
điều khiển công suất nhỏ trên một bo mạch (như các các ngõ ra của Vi Xử Lý).
Cách 2 là chỉ dùng 1 mosfet IRF540 thay vì 4 mosfet IRF 540, khơng đảo chiều
động cơ.
Lựa chọn cách 2.

II.2.2.4 Khối xử lý trung tâm :
Vi điều khiển Atmega16 là vi điều khiển 8bit dựa trên kiến trúc RISC.
Với khả năng thực hiện mỗi lệnh trong vịng một chu kỳ xung clock,
Atmega16 có thể đạt được tốc độ 1MIPS trên mỗi MHz (1 triệu lệnh /s/MHz),
các lệnh được xử lý nhanh hơn, tiêu thụ năng lượng thấp.
 Chức năng của Atmega16:
- Atmega16 có cấu trúc RISC với:
+ 131 lệnh, hầu hết được thực thi trong 1 chu kì xung nhịp.
+ 32x8 thanh ghi đa dụng.
+ Full static operaton.
+ Tốc độ làm việc 16MPIS, với thạch anh 16MHz.
+ Trong chip có 2 chức năng hỗ trợ gỡ rối và lập trình soạn chương trình.
- Bộ nhớ:
+ 16KB ISP plash với khả năng 10.000 lần ghi/xóa.
+ 512Byte EFROM.
+ 1KB SRAM ngoại.
- Giao tiếp JTAG
+ Khả năng qt tồn diện theo chuẩn JTAG.
+ Hỗ trợ lập trình FLASH, EEROM…
+ Hỗ trợ khả năng gỡ rối.
+ Lock bit qua giao tiếp JTAG.
- Ngoại vi:
+ 2 timer/counter 8 bit với các mode: so sánh và chia tần số.
+ 1 timer/counter 16 bit với các mode: so sánh, chia tần số, capture,
PWM.
+ 1 timer thời gian thực (Real time clock) với bộ dao động riêng biệt.
+ 4 kênh PWM
14

+ 8 kênh biến đổi ADC 10 bit.
+ Hỗ trợ giao tiếp 2IC
+ Bộ giao tiếp SPI.
+ Watch_dog timer với bộ dao động on-chip riêng biệt.
- Những thuộc tính đặc biệt:
+ Power on reset và brown-out detection.
+ chế độ hiệu chỉnh bộ sai số cho bộ dao động RC on-chip.
+ Các chế độ ngắt ngoài – trong đa dạng,
+ 6 mode sleep: Idle, ADC noise reduction , tiết kiệm năng lượng,
power-down, standby, extended stanby.
- I/O port: 40 chân I/O (Atmega16).
- Nguồn cấp: 2,7  5,5V
- Tiêu hao năng lượng:
+ Khi hoạt động tiêu thụ dòng 1,1 mA
+ Ở mode Idle tiêu thụ dòng 0,35mA.
+ Ở chế độ Power_down tiêu thụ dịng nhỏ hơn 1 µA.

Hình 10: Sơ đồ ngun lý vi điều khiển Atmega16
Tín hiệu được nhận từ cảm biến dò đường được đưa về 8 chân Port A của IC (Tích
hợp sẵn bộ chuyển đổi ADC trong chip).
Chip Atmega16 có 4 kênh PWM , sử dụng OCA1 (PIND .5) và OCB1(PIND.4)
làm 2 chân điều chế xung để điều khiển tốc độ 2 động cơ.
LCD được nối với PORT C của IC
15


CHƯƠNG III: XÂY DỰNG THUẬT TOÁN CHO ROBOT BÁM
ĐƯỜNG.
III.1 GIÁI THUẬT
III.1 Ý tưởng giải thuật

Khi xe hoạt động, sensor thu thập thơng tin qua bộ chuyển đổi ADC chuyển
thành tín hiệu điện và đưa về vi điều khiển. Giá trị nhận về sẽ được so sánh với 1 giá
trị đặt trước (ngưỡng). Nếu giá trị ADC[i]< giá trị ngưỡng thì LED thu thứ i đang ở
phần sân, ngược lại LED thu thứ i đang ở phần vạch.
Mạch cảm biến dùng 8 cặp led thu phát tương ứng với 8 giá trị ADC gửi về vi
điều khiển. Dựa trên 8 giá trị ADC thu về, xác định độ lệch tương đối giữa quỹ đạo
của robot và quỹ đạo mong muốn, sau đó so sánh độ lệch đó thành các mức.

Hình 11: Các mức lệch khỏi quỹ đạo của robot theo cảm biến dò đường.
Dựa trên các mức lệch, điều chỉnh tốc độ 2 bánh trái – phải để đưa robot về quỹ
đạo.
Cụ thể, để rẽ trái tốc độ bánh xe bên phải robot nhanh hơn bên trái một giá trị
tương ứng với các mức lệch (cần phải thử nghiệm nhiều lần), và ngược lại. Phương
pháp này đơn giản, nhưng robot chạy khơng ổn định, lúc rất nhanh, lúc lại chậm,
tính ổn định phụ thuộc nhiều vào động cơ và kết cấu cơ khí của robot.
Để khắc phục nhược điểm này, bằng cách áp dụng bộ điều khiển PID điều khiển vị
trí robot.
16


Giải thuật điều khiển PID
PID là cách viết tắt của các từ Propotional (tỉ lệ), Integral (tích phân) và
Derivative (đạo hàm). Tuy xuất hiện rất lâu nhưng đến nay PID vẫn là giải thuật
điều khiển được dùng nhiều nhất trong các ứng dụng điều khiển tự động. Để giúp
hiểu rõ hơn bản chất của giải thuật PID, hãy theo dõi ví dụ điều khiển vị trí của một
car (xe) trên đường thẳng. Giả sử bạn có một xe (đồ chơi...) có gắn một động cơ
DC. Động cơ sinh ra một lực để đẩy xe chạy tới hoặc lui trên một đường thẳng như
trong hình 12.

Hình 12. Ví dụ điều khiển vị trí xe trên đường thẳng

Gọi F là lực do động cơ tạo ra điều khiển xe. Ban đầu xe ở vị trí A, nhiệm vụ
đặt ra là điều khiển lực F (một cách tự động) để đẩy xe đến đúng vị trí O với các
yêu cầu: chính xác (accurate), nhanh (fast response), ổn định (small overshot).
Một điều rất tự nhiên, nếu vị trí hiện tại của xe rất xa vị trí mong muốn (điểm
O), hay nói cách khác sai số(error) lớn, chúng ta cần tác động lực F lớn để nhanh
chóng đưa xe về O. Một cách đơn giản để cơng thức hóa ý tưởng này là dùng quan
hệ tuyến tính:
F=Kp*e (1)
Trong đó Kp là một hằng số dương nào đó mà chúng ta gọi là hệ số P
(Propotional gain), e là sai số cần điều khiển tức khoảng cách từ điểm O đến vị trí
hiện tại của xe. Mục tiêu điều khiển là đưa e tiến về 0 càng nhanh càng tốt. Rõ ràng
nếu Kp lớn thì F cũng sẽ lớn và xe rất nhanh chóng tiến về vị trí O. Tuy nhiên, lực
F quá lớn sẽ gia tốc cho xe rất nhanh (định luật II của Newton: F=ma). Khi xe đã
đến vị trí O (tức e=0), thì tuy lực F=0 (vì F=Kp*e=F=Kp*0) nhưng do qn tính xe
vẫn tiếp tục tiến về bên phải và lệch điểm O về bên phải, sai số e lại trở nên khác 0,
giá trị sai số lúc này được gọi là overshot (vượt quá). Lúc này, sai số e là số âm, lực
F lại xuất hiện nhưng với chiều ngược lại để kéo xe về lại điểm O. Nhưng một lần
nữa, do Kp lớn nên giá trị lực F cũng lớn và có thể kéo xe lệch về bên trái điểm O.
Quá trình cứ tiếp diễn, xe cứ mãi dao động quanh điểm O. Có trường hơp xe dao
17


động càng ngày xàng xa điểm O. Bộ điều khiển lúc này được nói là khơng ổn định.
Một đề xuất nhằm giảm overshot của xe là sử dụng một thành phần “thắng” trong
bộ điều khiển. Sẽ rất lý tưởng nếu khi xe đang ở xa điểm O, bộ điều khiển sinh ra
lực F lớn nhưng khi xe đã tiến gần đến điểm O thì thành phần “thắng” sẽ giảm tốc
độ xe lại. Chúng ta đều biết khi một vật dao động quanh 1 điểm thì vật đó có vận
tốc cao nhất ở tâm dao động (điểm O). Nói một cách khác, ở gần điểm O sai số e
của xe thay đổi nhanh nhất (cần phân biệt: e thay đổi nhanh nhất không phải e lớn
nhất). Mặt khác, tốc độ thay đổi của e có thể tính bằng đạo hàm của biến này theo

thời gian. Như vậy, khi xe từ A tiến về gần O, đạo hàm của sai số e tăng giá trị
nhưng ngược chiều của lực F (vì e đang giảm nhanh dần). Nếu sử dụng đạo
hàm làm thành phần “thắng” thì có thể giảm được overshot của xe. Thành
phần “thắng” này chính là thành phần D (Derivative) trong bộ điều khiển PID mà
chúng ta đang khảo sát. Thêm thành phần D này vào bộ điều khiển P hiện tại,
chúng ta thu được bộ điều khiển PD nhu sau:
F=Kp*e + Kd*(de/dt) (2)
Trong đó (de/dt) là vận tốc thay đổi của sai số e và Kd là một hằng số không âm
gọi là hệ số D (Derivative gain).
Sự hiện diện của thành phần D làm giảm overshot của xe, khi xe tiến gần về O,
lực F gồm 2 thành phần Kp*e > =0 (P) và Kd*(de/dt) <=0 (D). Trong một số
trường hợp thành phần D có giá trị lớn hơn thành phần P và lực F đổi chiều,
“thắng” xe lại, vận tốc của xe vì thế giảm mạnh ở gần điểm O. Một vấn đề nảy sinh
là nếu thành phần D quá lớn so với thành phần P hoặc bản thân thành phần P nhỏ
thì khi xe tiến gần điểm O (chưa thật sự đến O), xe có thể dừng hẳn, thành phần D
bằng 0 (vì sai số e khơng thay đổi nữa), lực F = Kp*e. Trong khi Kp và e lúc này
đều nhỏ nên lực F cũng nhỏ và có thể khơng thắng được lực ma sát tĩnh. Bạn hãy
tưởng tượng tình huống bạn dùng sức của mình để đẩy một xe tải nặng vài chục
tấn, tuy lực đẩy tồn tại nhưng xe không thể di chuyển. Như thế, xe sẽ đứng yên mãi
dù sai số e vẫn chưa bằng 0. Sai số e trong tình huống này gọi là steady state error
(tạm dịch là sai số trạng thái tĩnh). Để tránh steady state error, người ta thêm vào bộ
điều khiển một thành phần có chức năng “cộng dồn” sai số. Khi steady state error
xảy ra, 2 thành phần P và D mất tác dụng, thành phần điều khiển mới sẽ “cộng
dồn” sai số theo thời gian và làm tăng lực F theo thời gian. Đến một lúc nào đó, lực
F đủ lớn để thắng ma sát tĩnh và đẩy xe tiến tiếp về điểm O. Thành phần “cộng
dồn” này chính là thành phần I (Integral - tích phân) trong bộ điều khiển PID. Vì
chúng ta điều biết, tích phân một đại lượng theo thời gian chính là tổng của đại
lượng đó theo thời gian. Bộ điều khiển đến thời điểm này đã đầy đủ là PID:
F=Kp*e + Kd*(de/dt)+Ki*∫edt
(3)

(chú ý: ∫edt là tích phân của biến e theo t)
18


Như vậy, chức năng của từng thành phần trong bộ điều khiển PID giờ đã
rõ. Tùy vào mục đích và đối tượng điều khiển mà bộ điều khiển PID có thể được
lượt bớt để trở thành bộ điều khiển P, PI hoặc PD. Cơng việc chính của người thiết
kế bộ điều khiển PID là chọn các hệ số Kp, Kd và Ki sao cho bộ điều khiển hoạt
động tốt và ổn định (quá trình này gọi là PID gain tuning). Đây khơng phải là việc
dễ dàng vì nó phụ thuộc vào nhiều yếu tố.
Kinh nghiệm chọn Kp, Kd, Ki được thực hành:
- Chọn Kp trước: thử bộ điều khiển P với đối tượng thật (hoặc mô phỏng),
điều chỉnh Kp sao cho thời gian đáp ứng đủ nhanh, chấp nhận overshot nhỏ.
- Thêm thành phần D để loại overshot, tăng Kd từ từ, thử nghiệm và chọn giá trị
thích hợp. Steady state error có thể sẽ xuất hiện.
- Thêm thành phần I để giảm steady state error. Nên tăng Ki từ bé đến lớn để
giảm steady state error đồng thời không để cho overshot xuất hiện trở lại.

19


III.2 Lưu đồ thuật toán:
a. Đọc ADC, so sánh với ngưỡng :
Start

for (i=0;iread_adc all
nguong = x
temp=read_adc[i]

Temp < nguong

S

Đ

adc[i]=temp

adc[i]=0

end

b. Xác định vị trí xe.
for
(i=0;isum1=sum1+adc[i]*(
i+1)
sum2=sum2+adc[i]

Start

S

sum2>0

Đ

temp1=0

temp1=sum1*100

temp1=temp1/sum2

End

return temp1-350

20


c. Bộ điều khiển PID điều khiển tốc độ động cơ:
Start

error = vitrixe()

Udk = PID_control ;

Udk==ma
x

Đ
D

am = Mid-Udk*10;
if (am<0) am = 0;
duong = Mid+Udk;

Đ
D

duong = Mid + 10*Udk;

if(duong<0){duong=0;}
am = Mid - Udk;

S
Udk== - max

S
am = Mid - Udk;
duong = Mid + Udk;

OCR1A = am;
OCR1B = duong;

End

21


III.2 CODE
/*****************************************************
This program was produced by the
CodeWizardAVR V2.05.0 Professional
Automatic Program Generator
© Copyright 1998-2010 Pavel Haiduc, HP InfoTech s.r.l.

Project :
Version :
Date : 1/2/2015
Author : NeVaDa
Company :

Comments:
Chip type
: ATmega16
Program type
: Application
AVR Core Clock frequency: 16.000000 MHz
Memory model
: Small
External RAM size
:0
Data Stack size
: 256
*****************************************************/
#include <mega16.h>
#include <delay.h>
#include <stdlib.h>
#include <stdio.h>
// Alphanumeric LCD Module functions
#include <alcd.h>
#define MAX(A,B) (A>B?A:B)
#define MIN(A,B) (A#define chanel 8
unsigned int adc[8];
unsigned char str[16];
unsigned char status[chanel] = {1,2,4,8,16,32,64,128};
unsigned char outStatus;
unsigned int tb=700 ;
int Max,Mid=300;
float error,old_error,error_sum, Kp=1.8, Ki=3,Kd=1;
float max_e;

// Timer 0 overflow interrupt service routine
interrupt [TIM0_OVF] void timer0_ovf_isr(void)
{
22


// Place your code here
}
#define ADC_VREF_TYPE 0x40
// Read the AD conversion result
unsigned int read_adc(unsigned char adc_input)
{
ADMUX=adc_input | (ADC_VREF_TYPE & 0xff);
// Delay needed for the stabilization of the ADC input voltage
delay_us(10);
// Start the AD conversion
ADCSRA|=0x40;
// Wait for the AD conversion to complete
while ((ADCSRA & 0x10)==0);
ADCSRA|=0x10;
return ADCW;
}
void read_adc_all()
{
unsigned char i;
unsigned int temp;
for (i=0;i{
temp=read_adc(i);
if (temp

{
adc[i]=0;
}
else
{
adc[i]=temp;
outStatus |= status[i];
}
}
}
//------------- TINH VI TRI XE---------int vitrixe()
{
char j;
unsigned int sum1=0, sum2=0;
float temp;
read_adc_all();
23


for (j=0; j{
sum1=sum1+adc[j]*(j+1);
sum2=sum2+adc[j];
}
if (sum2!=0)
{
temp = (float)sum1*100;
temp = temp/sum2;
}
else

temp=0;
return temp-450;
}
//------------PID------------------------------int PID_control()
{
float delta, Udk;
error =(float)vitrixe(); // Sai so dieu khien
delta = (error - old_error);
old_error = error;
//Luu gia tri sai so
error_sum = error_sum+error; // Thanh phan tich phan
// Khong cho thanh phan tich phan vuot qua gia tri max
if (error_sum <-max_e) error_sum = -max_e;
if (error_sum > max_e) error_sum = max_e;
Udk = Kp*error + Ki*error_sum + Kd*delta; //Tin hieu dieu khien
// Giam sat gia tri dieu khien ko duoc vuot qua ngwong
if (Udk <-Max) Udk = -Max;
if (Udk > Max) Udk = Max;
return (int)Udk;
}
void main(void)
{
// Declare your local variables here
int Udk;
int duong,am;
24


// Input/Output Ports initialization
// Port A initialization

// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In
Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTA=0x00;
DDRA=0x00;
// Port B initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In
Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTB=0x00;
DDRB=0x00;
// Port C initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In
Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTC=0x00;
DDRC=0x00;
// Port D initialization
// Func7=In Func6=In Func5=Out Func4=Out Func3=Out Func2=Out Func1=Out
Func0=Out
// State7=T State6=T State5=0 State4=0 State3=0 State2=0 State1=0 State0=0
PORTD=0x00;
DDRD=0x3F;
// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: 250.000 kHz
// Mode: Normal top=0xFF
// OC0 output: Disconnected
TCCR0=0x03;
TCNT0=0x00;

OCR0=0x00;
// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: 250.000 kHz
25