Thiết kế robot dò đường và tránh vật cản trong ma trận

Đề tài:
Thiết kế Robot dò đường và tránh vật cản
trong ma trận


MỤC LỤC
MỤC LỤC..................................................................................................................2
LỜI NÓI ĐẦU...........................................................................................................4
CHƯƠNG 1. TỔNG QUAN VỀ ĐỒ ÁN..................................................................5
I. GIỚI THIỆU VỀ HỆ THỐNG SỐ......................................................................5
1. Ngôn Ngữ VHDL………….…………………………………………...……5
2. Các Đặc Điểm ................................................................................................5
II. MÔ HÌNH ROBOT TỰ HÀNH…….................................................................8
III. MỤC TIÊU VÀ NỘI DUNG ĐỒ ÁN………………………………………...9
1. Mục tiêu.........................................................................................................9
2. Nội dung…………...……………………………………………………….9
CHƯƠNG 2. THIẾT KẾ ĐỒ ÁN............................................................................10
I. GIỚI THIỆU VỀ KIT CPLD COOLRUNNER – II 256 ....TQ144………….12
1. Lập trình trên kit………………………………………………………

12

II. THIẾT KẾ VÀ CHẾ TẠO ROBOT………………..………………………..15
1. Sơ đồ khối tổng thể Robot............................................................................15
2. Module siêu âm…………….........................................................................15
3. Module hồng ngoại……………....................................................................18
4. Module cầu H................................................................................................20
5. Module nguồn...............................................................................................22
6. Mô hình Robot thực tế..................................................................................22
CHƯƠNG III. LẬP TRÌNH ROBOT......................................................................24
2

I. THUẬT TOÁN DÒ ĐƯỜNG...........................................................................24
1. Xây dựng ma trận..........................................................................................24
2. Sơ đồ thuật toán............................................................................................25
II. CODE LẬP TRÌNH………….........................................................................27
CHƯƠNG IV. KẾT LUẬN......................................................................................32
I. ĐÁNH GIÁ – KẾT LUẬN………………………………………………..….32
II. HƯỚNG PHÁT TRIỂN CỦA ĐỒ ÁN…........................................................32

3


LỜI NÓI ĐẦU
Ngày nay điều khiển tự động đã trở thành một nhu cầu không thể thiếu được
của con người. Dưới sự xuất hiện của các học thuyết và các ứng dụng cụ thể trong
đời sống hằng ngày, có thể nói điều khiển tự động đang chi phối dần cuộc sống của
chúng ta. Con người đang cố gắng sáng tạo ra các con robot có khả năng làm việc
thay cho con người. Chúng ta thường bắt gặp các con robot trong các dây chuyền
công nghiệp sản xuất tự động hay robot giúp việc trong gia đình. Để tìm ra các ý
tưởng sáng tạo hay hàng năm đều diễn ra cuộc thi robocon châu á thái bình dương
đó là tiền đề để tạo ra những con robot có khả năng áp dụng vào thực tế.
Cũng chính vì mục đích đó mà chúng em thực hiện đồ án “Điều khiển robot
dò đường và tránh vật cản trong ma trận”, là bước khởi đầu trong lập trình robot để
robot có thể thực hiện các công việc tiếp theo.
Với công nghệ Logic số đã phát triển rất mạnh mẽ và là công nghệ chủ đạo
trong việc phát triển các sản phẩm điện-điện tử công nghiệp và dân dụng,điều khiển
tự động,viễn thông và công nghệ thông tin.Hiện nay có rất nhiều loại công nghệ
logic số khác nhau được sử dụng để thực hiện các thiết kế logic số.Một trong số đó
là công nghệ logic khả trình (Programmable Logic).Một cấu kiện logic có thể lập

trình được(PLD) là một IC số mà người dùng có thể cấu hình để chúng có khả năng
thực hiện các chức năng logic như mong muốn.Đây là 1 chip LSI có chứa 1 cấu
trúc “bình thường” và cho phép nhà thiết kế tạo tùy biến cho nó để dùng cho bất kì
một ứng dụng đặc biệt nào,tức là nó có thể được người dùng lập trình để thực hiện
một chức năng cần thiết cho ứng dụng của họ. Các PLD có các ưu điểm sau:
Thời gian thiết kế ứng dụng ngắn.
Chi phí phát triển thấp .
Giảm thiếu được yêu cầu khoảng trống trên bảng mạch.
Giảm thiểu được yêu cầu về điện.
Bảo đảm tính bảo mật của thiết kế.
Tốc độ chuyển mạch nhanh hơn.
Mật độ tích hợp cao .
Chi phí sản xuất số lượng lớn thấp.
PLD cũng cho phép nhà thiết kế có nhiều phương tiện linh động hơn để thí nghiệm
với các bản thiết kế bởi vì chúng có thể lập trình lại trong vài giây.
Với nhiều ưu điểm như vậy nên hiện nay có một số lượng lớn các PLD được các
nhà sản xuất IC tạo ra với nhiều tính năng đa dạng và nhiều tùy chọn có sẵn để nhà
thiết kế mạch có thể sử dụng một cách phổ biến.

4


CHƯƠNG 1. TỔNG QUAN VỀ ĐỒ ÁN
I.

GIỚI THIỆU VỀ HỆ THỐNG SỐ.

1. NGÔN NGỮ VHDL
VHDL là ngôn ngữ mô tả phần cứng cho các mạch tích hợp tốc độ rất
cao, là một loại ngôn ngữ mô tả phần cứng được phát triển dùng cho chương

trình VHSIC( Very High Speed Itergrated Circuit) của bộ quốc phòng Mỹ.
Mục tiêu của việc phát triển VHDL là có được một ngôn ngữ mô
phỏng phần cứng tiêu chuẩn và thống nhất cho phép thử nghiệm các hệ thống số
nhanh hơn cũng như cho phép dễ dàng đưa các hệ thống đó vào ứng dụng trong
thực tế.
Ngôn ngữ VHDL được ba công ty Intermetics, IBM và Texas
Instruments bắtđầu nghiên cứu phát triển vào tháng 7 năm 1983. Phiên bản đầu
tiên được công bố vào tháng 8-1985. Sau đó VHDL được đề xuất để tổ chức
IEEE xem xét thành một tiêu chuẩn chung. Năm 1987 đã đưa ra tiêu chuẩn về
VHDL( tiêu chuẩn IEEE-1076 1987).
2. CÁC ĐẶC ĐIỂM
VHDL được phát triển để giải quyết các khó khăn trong việc phát triển,
thay đổi và lập tài liệu cho các hệ thống số. VHDL là một ngôn ngữ độc lập
không gắn với bất kỳ một phương pháp thiết kế, một bộ mô tả hay công nghệ
phần cứng nào. Người thiết kế có thể tự do lựa chọn công nghệ, phương pháp
thiết kế trong khi chỉ sử dụng một ngôn ngữ duy nhất. Và khi đem so sánh với
các ngôn ngữ mô phỏng phần cứng khác ta thấy VHDL có một số ưu điểm hơn
hẳn là:
- Thứ nhất là tính công cộng:
VHDL được phát triển dưới sự bảo trợ của chính phủ Mỹ và hiện nay là một
tiêu chuẩn của IEEE. VHDL được sự hỗ trợ của nhiều nhà sản xuất thiết bị
cũng như nhiều nhà cung cấp công cụ thiết kế mô phỏng hệ thống.
- Thứ hai là khả năng được hỗ trợ bởi nhiều công nghệ và nhiều
phương pháp thiết kế:
VHDL cho phép thiết kế bằng nhiều phương pháp ví dụ phương pháp thiết kế
từ trên xuống, hay từ dưới lên dựa vào các thư viện sẵn có. VHDL cũng hỗ trợ
5


cho nhiều loại công cụ xây dựng mạch như sử dụng công nghệ đồng bộ hay

không đồng bộ, sử dụng ma trận lập trình được hay sử dụng mảng ngẫu nhiên.
- Thứ ba là tính độc lập với công nghệ:
VHDL hoàn toàn độc lập với công nghệ chế tạo phần cứng. Một mô tả hệ thống
dùng VHDL thiết kế ở mức cổng có thể được chuyển thành các bản tổng hợp
mạch khác nhau tuỳ thuộc công nghệ chế tạo phần cứng mới ra đời nó có thể
được áp dụng ngay cho các hệ thống đã thiết kế.
- Thứ tư là khả năng mô tả mở rộng:
VHDL cho phép mô tả hoạt động của phần cứng từ mức hệ thống số cho
đến mức cổng. VHDL có khả năng mô tả hoạt động của hệ thống trên nhiều
mức nhưng chỉ sử dụng một cú pháp chặt chẽ thống nhất cho mọi mức. Như thế
ta có thể mô phỏng một bản thiết kế bao gồm cả các hệ con được mô tả chi tiết.
- Thứ năm là khả năng trao đổi kết quả:
Vì VHDL là một tiêu chuẩn được chấp nhận, nên một mô hình VHDL
có thể chạy trên mọi bộ mô tả đáp ứng được tiêu chuẩn VHDL. Các kết quả mô
tả hệ thống có thể được trao đổi giữa các nhà thiết kế sử dụng công cụ thiết
kếkhác nhau nhưng cùng tuân theo tiêu chuẩn VHDL. Cũng như một nhóm
thiết kế có thể trao đổi mô tả mức cao của các hệ thống con trong một hệ thống
lớn (trong đó các hệ con đó được thiết kế độc lập).
- Thứ sáu là khả năng hỗ trợ thiết kế mức lớn và khả năng sử dụng lại
các thiết kế:
VHDL được phát triển như một ngôn ngữ lập trình bậc cao, vì vậy nó có thể
được sử dụng để thiết kế một hệ thống lớn với sự tham gia của một nhóm nhiều
người. Bên trong ngôn ngữ VHDL có nhiều tính năng hỗ trợ việc quản lý, thử
nghiệm và chia sẻ thiết kế. Và nó cũng cho phép dùng lại các phần đã có sẵn.

6


II.

MÔ HÌNH ROBOT TỰ HÀNH.
Một trong các ứng dụng của “Hệ Thống Số” là mô hình Robot tự hành hay

robot di động (mobile robot hay được viết tắt là mobot), được định nghĩa là một
loại xe robot có khả năng tự dịch chuyển, tự vận động (có thể lập trình lại được)
dưới sự điều khiển tự động có khả năng hoàn thành công việc được giao.
Theo lý thuyết, môi trường hoạt động của robot tự hành có thể là đất,
nước, không khí, không gian vũ trụ hay tổ hợp giữa chúng. Địa hình bề mặt mà
robot di chuyển trên đó có thể bằng phẳng hoặc thay đổi, lồi lõm.
Theo bộ phận thực hiện chuyển động, ta có thể chia robot tự hành thành 2
loại: chuyển động bằng chân và chuyển động bằng bánh.

a)

b)
Hình 1. Robot tự hành

7


III. MỤC TIÊU VÀ NỘI DUNG ĐỒ ÁN
1. Mục tiêu
Đồ án này đi sâu nghiên cứu, chế tạo Robot tự hành di chuyển bằng bánh, dò
đường theo vạch trắng, tránh vật cản và tìm được đường về đích trong ma trận, sử
dụng cảm biến hồng ngoại và cảm biến siêu âm SRF05, được xử lý và điều khiển
bởi kit Cool Runner II
2. Nội dung
Nội dung nghiên cứu bao gồm những phần sau:
- Tìm hiểu về KIT COOL RUNNER II
- Xây dựng sơ đồ khối tổng thể cho Robot

- Thiết kế, chế tạo các bộ phận(Module) của Robot
- Xây dựng thuật toán và lập trình cho Robot
- Thử nghiệm, kiểm tra lỗi
- Kết luận: Đánh giá và hướng phát triển

8


CHƯƠNG 2. THIẾT KẾ ĐỒ ÁN

I.

Giới thiệu về kit thử nghiệm CPLD COOLRUNNER – II 256 TQ144

The CooRunner – II 256 TQ144 là một trong những bộ kit CPLD của xilinx.
Bộ kit bao gồm nguồn cung cấp với hiệu quả cao,bộ tạo dao động có thể cấu hình ,
các cổng giao tiếp I/0,đồng hồ đo thời gian thực và cổng USB cấp nguồn và lập
trình cho CPLD .Bộ KIT có 5 cổng mở rộng với 64 tín hiệu giao tiếp từ CPLD ra
mạch ngoài giúp mở rộng khả năng kết nối.
Các đặc điểm nổi bật của KIT:
256 khối Coolrunner – II CPLD trong gói TQ 144
Có cổng USB cấp nguồn ,lập trình và chuyển dữ liệu người dung
Bộ dao động có thể thay đổi được (1000/100/10khz) ,them vào đó là có khe
nắp bộ dao động thạch anh thứ 2, 64 tín hiệu I/0 trên các cổng giao tiếp(32 trên các
cổng nối tiếp ,32 trên các cổng song song)
Các miếng đệm cho SPI PROM gắn trong

9

Hình 2. CoolRunner – II 256 TQ144

Hình 3. Sơ đồ khối của KIT CPLD CoolRunner – II 256 TQ144

10


3.2.Lập trình trên KIT thử nghiệm
Modul điều khiển được viết trên VHDL – một ngôn ngữ mô tả phần cứng của
Xilinx sử dụng phần mềm Xilinx Integrated Software Environment (ISE) v.10.0 .
Việc đầu tiên là soạn thảo văn bản có sẵn trong ISE để viết modul và hệ thống quản
lí project ,file cấu hình cho CoolRunner – II 256 TQ144 được tạo ra trong ISE .Nó
bao gồm những việc sau (tất cả đều được phần mềm làm tự động ) :
Biên dịch VHDL thành sơ đồ các cổng logic ,thành phần của mạch (thông qua công
cụ tổng hợp XST)
Vẽ sơ dựa trên những tài nguyên có sẵn trên FPGA
Sắp xếp các thành phần để đạt sự tối ưu nhất
File cấu hình được nạp vào CPLD qua giao tiếp JTAG .Kết quả của quá trình tối ưu
hóa có thể dẫn đến sự thay đổi các file gốc và các chu trình sau đó.

II. THIẾT KẾ VÀ CHẾ TẠO ROBOT
1. Sơ đồ khối tổng thể Robot

MODULE
SIÊU ÂM
MODULE
HỒNG NGOẠI

BỘ XỬ LÝ
TRUNG TÂM

KHỐI NGUỒN
11

KHỐI
ĐỘNG CƠ


Hình 7. Sơ đồ khối Robot
2. Module siêu âm
Sử dụng module siêu âm SRF05.

Hình 8. Cảm biến siêu âm SRF05
SRF05 là một bước phát triển từ SRF04, được thiết kế để làm tăng tính linh
hoạt, tăng pham vi, ngoài ra còn giảm bớt chi phí. Khoảng cách đo từ 3 mét đến 4
mét. Một chế độ hoạt động mới, SRF05 cho phép sử dụng một chân duy nhất cho
cả kích hoạt và phản hồi, do đó tiết kiệm có giá trị trên chân điều khiển của bạn.
Khi chân chế độ không kết nối, SRF05 các hoạt động riêng biệt chân kích hoạt và
và chân hồi tiếp. SRF05 bao gồm một thời gian trễ trước khi xung phản hồi để
mang lại điều khiển chậm hơn chẳng hạn như bộ điều khiển thời gian cơ bản
Stamps và Picaxe để thực hiện các xung lệnh
Chế độ 1:
Tương ứng SRF04 – tách biệt kích hoạt và phản hồi. Chế độ này sử dụng
riêng biệt chân kích hoạt và chân phản hồi và là chế độ đơn giản nhất để sử dụng.
Để sử dụng chế độ này, chỉ cần chân chế độ không kết nối – SRF05 có một nội
dừng trên chân này. Từ đó ta nhận thấy: Để cho SRF05 hoạt động thì cần cấp 1
xung mức cao có độ rộng tối thiếu 10us trên chân Trig. Sau khi nhận được xung từ
12

chân Trig thì srf05 sẽ tạo ra 8 xung để phát siêu âm, sau khi hoàn thành việc phát 8
xung này thì srf05 sẽ kéo chân Echo lên mức 1, độ rộng của mức 1 trên chân Echo
tương ứng với khoản cách của vật cản với srf05, nếu ko có vật cản thì nó sẽ được
trả về mức 0 sau 30ms. Đặc biệt là srf05 chỉ có thể nhận xung trên chân Trig tối đa
là 20Hz, cho nên việc kích xung trên chân Trig phải phù hợp thì srf05 mới hoạt
động chính xác.

Hình 9. SRF05 hoạt động ở chế độ 1
13


Chế độ 2:
Dùng một chân cho cả kick hoạt và phản hồi. Chế độ này sử dụng một chân
duy nhất cho cả tín hiệu kích hoạt và hồi tiếp, và được thiết kế để lưu các giá trị
trên chân lên bộ điều khiển nhúng. Để sử dụng chế độ này, chân chế độ kết nối vào
chân mát. Tín hiệu hồi tiếp sẽ xuất hiện trên cùng một chân với tín hiệu kích hoạt.
SRF05 sẽ không tăng dòng phản hồi cho đến 700uS sau khi kết thúc các tín hiệu
kích hoạt. Bạn đã có thời gian để kích hoạt pin xoay quanh và làm cho nó trở thành
một đầu vào và để có pulse đo mã của bạn đã sẵn sàng. Lệnh PULSIN được tìm ra
và được dùng phổ biến hiện nay để điều khiển tự động.

Hình 10. SRF05 hoạt động ở chế độ 2

14


Để sử dụng chế độ 2 với các Stamps BS2 cơ bản, bạn chỉ cần sử dụng
PULSOUT và PULSIN trên cùng một chân:
SRF05 PIN 15 sử dụng cho cả hai và kick hoạt Echo
Range VAR Word xác định phạm vi biến 16 bit.

SRF05 = 0 bắt đầu bằng pin thấp
PULSOUT SRF05 5 đưa ra kick hoat pulse 10us
PULSIN SRF0, 1, range Echo đo thời gian
Range = range/29 để chuyển đổi sang cm
Tính khoảng cách:
Giản đồ định thời SRF05 thể hiện trên đây cho mỗi chế độ. Bạn chỉ cần cung
cấp một đoạn xung ngắn 10uS kích hoạt đầu vào để bắt đầu đo khoảng cách. Các
SRF05 sẽ gửi cho ra một chu kỳ 8 burst của siêu âm ở 40khz và tăng cao dòng phản
hồi của nó (hoặc kích hoạt chế độ dòng 2). Sau đó chờ phản hồi, và ngay sau khi
phát hiện nó giảm các dòng phản hồi lại. Dòng phản hồi là một xung có chiều rộng
là tỷ lệ với khoảng cách đến đối tượng. Bằng cách đo xung, ta hoàn toàn có thể để
tính toán khoảng cách ttheo inch / centimét hoặc bất cứ điều gì khác. Nếu
không phát hiện gì cả SRF05 giảm thấp hơn dòng phản hồi của nó sau khoảng
30ms. SRF04 cung cáp một xung phản hồi tỷ lệ với khoang cách. Nếu độ rộng của
pulse được đo trong hệ uS, sau đó chia cho 58 sẽ cho khoảng cách theo cm, hoặc
chia cho 148 sẽ cho khoảng cách theo inch. Us/58= cm hay us/148= inch.
SRF05 có thể được kích hoạt nhanh chóng với mọi 50mS, hoặc 20 lần mỗi
giây. Bạn nên chờ 50ms trước khi kích hoạt kế tiếp, ngay cả khi SRF05 phát hiện
một đối tượng gần và xung phản hồi ngắn hơn. Điều này là để đảm bảo các siêu âm
“beep” đã phai mờ và sẽ không gây ra sai phản hồi ở lần đo kế tiếp.

15


3. Module hồng ngoại
Ở đồ án này, chúng em sử dụng ma trận 2x3 vạch trắng nền đen. Vì vậy,
Robot sẽ phải dò đường bám theo vạch trắng. Và để dò đường thì chúng em sử
dụng cảm biến hồng ngoại(1 mắt phát và 1 mắt thu), phát hiện vạch trắng – đen.

Phá

t
Phản xạ
lại ít

Thu

Phản xạ
lại nhiều

Mức 1(2V)

Mức 0(0V)

Hình 11. Nguyên lý phát hiện vạch trắng – đen
Khi gặp vạch trắng ánh sáng hồng ngoại phát ra từ mắt phát sẽ phản xạ lại
mắt thu với cường độ mạnh, và có tín hiệu điện ra khoảng gần 2V ta coi là mức
logic 1. Còn khi gặp vạch đen, ánh sáng hồng ngoại từ mắt phát phát ra sẽ bị vạch
đen hấp thụ, lượng ánh sáng hồng ngoại phản xạ lại mắt thu ít, cường độ yếu khi đó
không có tín hiệu điện ra (0V) ta coi là mức logic 0.
Do tín hiệu điện áp ra từ cảm biến hồng ngoại yếu, nên ta đưa qua mạch so
sánh sử dụng IC khuếch đại thuật toán LM339, so sánh tín hiệu điện áp đưa vào từ
cảm biến hồng ngoại với 1 giá trị điện áp do ta chỉnh từ biến trở, ở đây đặt giá trị
đó vào khoảng 1V, nếu điện áp vào >1V thì đưa ra mức logic 0(0V), <1V thì đưa ra
mức logic 1(5V).

16


Hình12.IC LM339
Khi đó, gặp vạch trắng thì sẽ cho ra mức 0 (0V), gặp vạch đen thì sẽ cho ra mức

1(5V). Mà Kit Arm có điện áp hoạt động từ 2 – 3.6V nên ta phải cho tín hiệu hồng
ngoại qua mạch phân áp 3.3V để khi gặp vạch đen tín hiệu ra là 5V, qua mạch phân
áp sẽ còn khoảng 3.3V để đưa vào Kit nhận mức 1, còn gặp vạch trắng thì vẫn là
0V đưa vào Kit nhận mức 0.
Dưới đây là hình ảnh Module hồng ngoại đã tích hợp IC LM339, gồm 4 cặp
thu phát hồng ngoại.

17


Hình 13. Module hồng ngoại tích hợp IC LM339
4. Module cầu H

Hình 14. Module cầu H dùng IC L298
Sử dụng IC L298 điều khiển 2 động cơ.
IC L298 là một IC tích hợp nguyên khối gồm 2 mạch cầu H bên trong.
Với điện áp làm tăng công suất đầu ra từ 5V – 47V, dòng ra 2A, L298 rất thích hợp
trong những ứng dụng công suất nhỏ như động cơ DC loại vừa.
Chức năng các chân của L298:
- 4 chân INPUT: IN1, IN2, IN3, IN4 được nối lần lượt với các chân 5, 7, 10,
12 của L298. Đây là các chân nhận tín hiệu điều khiển.
- 4 chân OUTPUT: OUT1, OUT2, OUT3, OUT4 (tương ứng với các chân
18


INPUT) được nối với các chân 2, 3, 13, 14 của L298. Các chân này sẽ được nối với
động cơ.
- Hai chân ENA và ENB dùng để điều khiển các mạch cầu H trong L298.
Nếu ở mức logic “1” (nối với nguồn 5V) thì cho phép mạch cầu H hoạt động, nếu ở
mức logic “0” thì mạch cầu H không hoạt động.

Cách điều khiển động cơ:
- Khi ENA, ENB = 0: Động cơ 1, 2 không quay với mọi đầu vào .
- Khi ENA = 1:
INT1 = 1; INT2 = 0: động cơ 1 quay thuận.
INT1 = 0; INT2 = 1: động cơ 1 quay nghịch.
INT1 = INT2: động cơ 1 dừng ngay tức thì.
- Khi ENB = 1:
INT3 = 1; INT4 = 0: động cơ 2 quay thuận.
INT3 = 0; INT4 = 1: động cơ 2 quay nghịch.
INT3 = INT4: động cơ 2 dừng ngay tức thì.

5. Module nguồn
Nguồn5V:
Sử dụng cho Kit Arm và 2 module cảm biến.

19


Điện áp vào 12VDC từ Pin cho qua cầu diode để tránh được lỗi khi mắc nhầm 2
cực +, - của Pin.

Nguồn điện áp ra điều chỉnh được:
Điện áp vào 12VDC – Điện áp ra từ 1,25V-10V
Sử dụng cho động cơ DC.

6. Mô hình Robot thực tế
Sau khi lắp rắp các module trên Robot được sản phẩm hoàn thiện gồm 1
khung xe có gắn 2 động cơ DC ghép với 2 bánh xe, 2 cặp cảm biến hồng ngoại đặt
20

phía trước xe để giúp Robot bám vạch, 2 cặp cảm biến hồng ngoại lắp 2 bên xe để
phát hiện các ngã 3 ngã 4 và trường hợp về đích, 1 cảm biến siêu âm đặt phía trước
xe để phát hiện vật cản, mạch nguồn và mạch cầu H được gắn ở tầng dưới khung
xe, mạch hồng ngoại và Kit Coolruner II được gắn ở tầng trên khung xe
CHƯƠNG III. LẬP TRÌNH ROBOT
I. THUẬT TOÁN DÒ ĐƯỜNG
1. Xây dựng ma trận

Hình 16. Ma trận đường đi
Từ ma trận trên ta thấy được đường đi và các ngã rẽ cần phải thiết lập cho
Robot nhờ vào các cảm biến hồng ngoại gồm có:

21


- Ngã 3 dọc: là trường hợp 1 cảm biến hồng ngoại bên trái nhận mức 1 (vạch
đen), 2 cảm biến hồng ngoại phía trước và 1 bên phải nhận mức 0 (vạch trắng).
- Ngã 3 ngang: là trường hợp 2 cảm biến hồng ngoại phía trước nhận mức 1,
2 cảm biến hồng ngoại 2 bên nhận mức 0.
- Ngã 4: là trường hợp 1 trong 2 cảm biến hồng ngoại phía trước nhận mức 0
và 2 cảm biến hồng ngoại 2 bên cũng nhận mức 0.
- Đích: là trường hợp chỉ 1 trong 2 cảm biến hồng ngoại 2 bên nhận mức 1
và 2 cảm biến hồng ngoại phía trước nhận mức 1.
2. Sơ đồ thuật toán

Hình 17. Sơ đồ thuật toán

Ban đầu robot xuất phát tại điểm đầu đã cho trước và đích đã xác định , sau
đó đi thẳng nếu modul hồng ngoại trước nhận được tín hiệu L1L2=”11” , nếu

22


không gặp vật cản nó sẽ tiến về phía trước cho tới khi gặp ngõ cụt , nếu có vật cản
robot lùi lại , gặp ngã tư khi L3 L4 =”11” robot quay phải , và đi thẳng , gặp ngã tư
tiếp theo rẽ trái và cứ nhứ thế xét từng ô 1 phía bên trên , nếu k có robot sẽ tiến về
ngõ cụt sau đó rẽ phải và về đích , robot dừng lại

II. CODE LẬP TRÌNH
---------------------------------------------------------------------------------library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use ieee.std_logic_unsigned.all;
use ieee.std_logic_arith.all;
entity test2 is
port ( clkcpld : in std_logic;
ech : in std_logic;
trigger : out std_logic;
led : out std_logic);
end test2;
architecture beh of test2 is
component machtonghop is
Port ( clkcpld : in std_logic;
ech : in std_logic;
trigger : out std_logic;
distanceout : out std_logic_vector(8 downto 0));
end component;
component test is
port( pulse : in std_logic_vector(8 downto 0);
led : out std_logic);
end component;

signal pulseout :std_logic_vector(8 downto 0);
begin
U6
:
machtonghop
port
map(clkcpld
=>
23

clkcpld,ech


=>ech,trigger=>trigger,distanceout=>pulseout);
U7 : test port map(pulse=>pulseout,led=>led);
end beh;
-- khoi tao tan so-library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
use ieee.std_logic_arith.all;
entity tanso is
Port (
clk : in STD_LOGIC; -- tan so thach anh tu kit CPLD 8MHz
clk_1us :out std_logic;
clk_58us : out std_logic
);
end tanso;
architecture Beh of tanso is
signal dem1us : integer range 7 downto 0;
signal dem58us: integer range 463 downto 0;

begin
-- tao xung 1us
tao_clk1us :process (clk)
begin
if(clk'event and clk = '1') then
if dem1us= 7 then dem1us <=0;
clk_1us <='1';
else
dem1us <= dem1us + 1;
clk_1us<='0';
end if;
end if;
end process tao_clk1us;
tao_clk58us :process (clk)
begin
if(clk'event and clk = '1') then
if dem58us= 463 then dem58us <=0;
clk_58us <='1';
else
dem58us <= dem58us + 1;
24


clk_58us<='0';
end if;
end if;
end process tao_clk58us;
end beh;
--- dem xung 1us
library IEEE;

use IEEE.STD_LOGIC_1164.ALL;
use ieee.std_logic_unsigned.all;
use ieee.std_logic_arith.all;
entity demxung is
generic(n:positive:=10);
Port( clk : in std_logic;
en
: in std_logic;
reset :in std_logic;
output : out std_logic_vector(n-1 downto 0));
end demxung;
architecture beh of demxung is
signal num :std_logic_vector(n-1 downto 0);
begin
process(reset,clk)
begin
if(reset='0') then
num<= (others=>'0');
elsif(clk'event and clk='1') then
if(en='1') then
num<=num+1;
end if;
end if;
end process;
output <= num;
end beh;
-- kich hoat trigger
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use ieee.std_logic_unsigned.all;

use ieee.std_logic_arith.all;
entity kichhoat_trigg is
Port( clk_1us : in std_logic;
25