HƯỚNG DẪN TÌM HIỂU CHƯƠNG TRÌNH TDC MCR doc

2012
Tran Hong Van
Microsoft
10/1/2012
HƯỚNG DẪN TÌM HIỂU
CHƯƠNG TRÌNH TDC MCR

MỤC LỤC
CHƢƠNG 1: ĐẶT TẢ CÁC MODULE CỦA XE TDC MCR
1.1 Giao diện kết nối 1
1.2 Board cảm biến dò đƣờng: 2
1.3 Board xử lý sensor: 3
1.4 Board công suất 2 DC Motor L298: 4
1.5 Boar vi điều khiển PIC16F887: 6
CHƢƠNG 2: ĐIỀU KHIỂN MODULE CỦA XE TDC MCR
2.1 Khối LED 9
2.2 Khối Loa Beep 10
2.3 Khối DipSwitch 11
2.4 Khối nút nhấn. 12
2.5 Khối RC Servo 13
2.6 Khối động cơ DC 14
2.7 Khối Xử lý sensor 16
CHƢƠNG 3: XÂY DỰNG GIẢI THUẬT ĐIỀU KHIỂN
3.1 Cấu trúc chƣơng trình 17
3.2 Giải thuật xử lý khi qua các đoạn đƣờng thẳng và cong. 19
3.3 Giải thuật qua đoạn đƣờng 900
0
22
3.4 Giải thuật chuyển làn bên phải 23
3.5 Các lỗi có thể xảy ra trên đƣờng đua 25

CHƢƠNG 4: CÀI ĐẶT CÔNG CỤ LẬP TRÌNH
4.1 Cài đặt phần mềm MPLAB 33
4.2 Mở dự án mẫu 40
CHƢƠNG 5: CHƢƠNG TRÌNH MẪU MCR
5.1 Mã chƣơng trình 42
5.2 Giải thích một số đoạn code trong chƣơng trình 53
CHƢƠNG 6: NẠP CHƢƠNG TRÌNH VÀ SỬ DỤNG XE TDC MCR

MỤC LỤC HÌNH ẢNH
Hình 1 - 1: Vị trí các mạch điện trên xe TDC MCR 1
Hình 1 - 2: Sơ đồ khối kết nối các mạch trên TDC MCR 1
Hình 1 - 3: Hình dáng board dò đường 2
Hình 1 - 4: Nguyên lý 1 đầu dò cảm biến 2
Hình 1 - 5: Nguyên lý phản xạ ánh sáng 2
Hình 1 - 6 : Hình dáng board xử lý dò đường 3
Hình 1 - 7: Nguyên lý 1 kênh xử lý cảm biến 3
Hình 1 - 8: Hình dáng board công suất 2 DC Motor L298 4
Hình 1 - 9: Nguyên lý board công suất 2 DC Motor L298 5
Hình 1 - 10: Bảng trạng thái tín hiệu điều khiển 1 kênh DC Motor 5
Hình 1 - 11: Hình dáng board vi điều khiển 6
Hình 1 - 12: Sơ đồ nguyên lý board vi điều khiển PIC16F887 cho xe MCR. 7
Hình 2- 1: Nguyên lý Led 9
Hình 2- 2: Các loại led thông dụng 9
Hình 2- 3: 2 Led trên board 9
Hình 2- 4: Sơ đồ nguyên lý khối Led 10
Hình 2- 5: Buzzer thực tế trên board 10
Hình 2- 6: Sơ đồ nguyên lý điều khiển Buzzer 10
Hình 2- 7: Dipswitch thực tế 11
Hình 2- 8: Sơ đồ nguyên lý dipswitch 4 11
Hình 2- 9: Dipswitch 4 trên board 11

Hình 2- 10: Sơ đồ nguyên lý khối nút nhấn 12
Hình 2- 11: Nút nhấn thực tế trên board 12
Hình 2- 12: Tín hiệu rung nhiễu khi nhấn nút 13
Hình 2- 13: RC Servo 14
Hình 2- 14: Tín hiệu PWM điều khiển RC Servo 14
Hình 2- 15: Động cơ DC 15

Hình 2- 16: Điều khiển chiều quay động cơ điện một chiều 15
Hình 2- 17: PWM điều khiển tốc độ động cơ 16
Hình 2- 18: Khối sensor 16
Hình 3- 1: Sơ đồ giải thuật 17
Hình 3- 2: Sensor bắt được nửa line bên trái 17
Hình 3- 3: Chạy qua đoạn chuyển làn trái 18
Hình 3- 4: Sensor bắt được nửa line bên phải 18
Hình 3- 5: Chạy qua đoạn chuyển làn trái 18
Hình 3- 6: Sensor bắt được nguyên line 19
Hình 3- 7: Chạy xong đoạn đường cua 90
0
19
Hình 3- 8: Sensor báo hiệu vị trí giữa line 21
Hình 3- 9: Sensor báo hiệu lệch nhỏ về phía bên trái so với vị trí trung tâm 21
Hình 3- 10: Sensor báo hiệu lệch về phía bên trái so với vị trí trung tâm 22
Hình 3- 11: Các bước qua đoạn đường 90
0
22
Hình 3- 12: Các bước qua vị trí line cắt ngang 23
Hình 3- 13: Các bước chuyển làn bên phải 24
Hình 3- 14: Các bước qua vị trí nửa line cắt ngang 24
Hình 3- 15: Vị trí queo trái – xe vượt qua đường đua 25
Hình 3- 16: Vị trí xác định line cắt ngan, xe bị lệch 26

Hình 3- 17: Sensor bắt nữa line bên phải 26
Hình 3- 18: Xe không quẹo một cách chính xác 26
Hình 3- 19: Các trạng thái sensor khi quẹo 27
Hình 3- 20: Vị trí chuyển làn xe tiếp tục đi về phía trước 28
Hình 3- 21: Các trạng thái sensor trước khi chuyển làn 28
Hình 3- 22: Chạy theo line biên của đường đua 29
Hình 3- 23: Trạng thái quẹo bám line trung tâm và line biên 29
Hình 3- 24: Trạng thái sensor thay đổi chính xác 30
Hình 3- 25: Trạng thái sensor thay đổi không chính xác 30

Hình 3- 26: Thay đổi trạng thái 0x83, 0x81, 0xC1 30
Hình 3- 27: Các trạng thái chuyển line biên về line trung tâm 31
Hình 3- 28: Xe chuyển làn sau đó vượt khỏi đường đua 31
Hình 3- 29: Trạng thái chuyển làn đúng và không đúng 32
Hình 4. 1: Click file setup 33
Hình 4. 2: Nạp chương trình cài đặt 33
Hình 4. 3: Đồng ý tiếp tục cài đặt phần mềm 34
Hình 4. 4: Chấp nhận các điều khoản khi sử dụng phần mềm 34
Hình 4. 5: Lựa chọn cách cài đặt 34
Hình 4. 6: Lựa chọn thư mục chứa phần mềm 35
Hình 4. 7: Chấp nhận cài đặt ứng dụng Maestro 35
Hình 4. 8: Chấp nhận cài đặt ứng dụng MPLAB C32 35
Hình 4. 9: Xem lại các lựa chọn cài đặt 36
Hình 4. 10: Tiến trình cài đặt 36
Hình 4. 11: Cài đặt HI – TECH C? 36
Hình 4. 12: Tiếp tục cài đặt phần mềm hỗ trợ HI – TECH C 37
Hình 4. 13: Chấp nhận cài đặt HI – TECH C 37
Hình 4. 14: Lựa chọn thư mục cài đặt HI – TECH C 38
Hình 4. 15: Chọn ngôn ngữ sử dụng trong HI – TECH C 38
Hình 4. 16: Quá trình cài đặt HI – TECH C 38

Hình 4. 17: Hoàn thành quá trình cài đặt HI-TECH C 39
Hình 4. 18: Hoàn thành quá trình cài đặt MPLAB 39
Hình 4. 19: Các hướng dẫn MPLAB IDE 39
Hình 4. 20: Mở dự án mẫu 40
Hình 4. 21: Code của dự án mẫu 40
Hình 4. 22: Biên dịch dự án mẫu 41
Hình 5. 1: Lưu đồ giải thuật do line 58
Hình 6. 1: Nối nguồn cho xe 59

Hình 6. 2: Mở nguồn cho xe 59
Hình 6. 3: Báo pin yếu 60
Hình 6. 4: Sạc pin 60
Hình 6. 5: Nối một đầu USB vào máy tính. 61
Hình 6. 6: Nối 1 đầu USB vào mạch nạp 61
Hình 6. 7: Nối mạch nạp với board vi điều khiển pic16f887 61
Hình 6. 8: Mạch nạp kết nối thành công 62
Hình 6. 9: Mạch nạp kết nối không thành công 62
Hình 6. 10: Biên dịch thành công 63
Hình 6. 11: Nạp dữ liệu xuống vi điều khiển 63



1

CHƢƠNG 1: ĐẶT TẢ CÁC MODULE CỦA XE TDC MCR
1.1. Giao diện kết nối


Hình 1 - 1: Vị trí các mạch điện trên xe TDC MCR

Xe TDC MCR bao gồm board cảm biến dò đường, board xử lý cảm biến, board MCU
PIC16F887, board công suất 2 DC Motor L298 kết hợp với nhau để điều khiển sự di
chuyển của xe, tức là điều khiển hệ thống động cơ trái, phải và động cơ rc servo.








Hình 1 - 2: Sơ đồ khối kết nối các mạch trên TDC MCR

Mạch dò
đường


VĐK PIC 16F887


Buttons
DIPSW4


Mạch xử
lý sensor


Led
Buzzer

Mạch công suất

Động
cơ trái

Động
cơ phải


RC
Servo

Mạch dò đường
RC Servo
Mạch VĐK PIC 16F887
Mạch xử lý sensor
DC Motor
Bus

2

1.2. Board cảm biến dò đƣờng:
a. Hình dáng:

Hình 1 - 3: Hình dáng board dò đường
b. Sơ đồ nguyên lý 1 đầu dò cảm biến:

Hình 1 - 4: Nguyên lý 1 đầu dò cảm biến

c. Nguyên lý hoạt động:
Board cảm biến dò đường gồm 8 đầu dò sensor, mỗi đầu dò có nguyên lý như hình 1-
4. Nguồn cấp VCC=5V lấy từ board xử lý cảm biến, led phát là led siêu sáng phát ánh
sáng trắng, phần thu dùng quang trở , qua cầu phân áp R19 và PR1 tín hiệu ra là áp
thay đổi tùy thuộc vào lượng ánh sáng phản xạ về (theo hình 1-5). Tín hiệu ra IN1
được đưa đến mạch xử lý cảm biến.

Hình 1 - 5: Nguyên lý phản xạ ánh sáng


3

1.3. Board xử lý sensor:
a. Hình dáng:

Hình 1 - 6 : Hình dáng board xử lý dò đường
b. Sơ đồ nguyên lý 1 kênh xử lý sensor:




Hình 1 - 7: Nguyên lý 1 kênh xử lý cảm biến
c. Nguyên lý hoạt động:
Tín hiệu IN1 từ đầu dò sensor được đưa đến ngõ vào (-) để so sánh với với ngõ vào
(+) của opam LM324. Tại ngỏ vào (+) luôn có 1 điện áp V+ được thay đổi bởi biến trở
100K. Nếu sensor được đưa đến nền màu trắng thì lượng ánh sáng phản xạ về nhiều,
điện trở quang trở sẽ nhỏ làm điện áp IN1 cũng nhỏ theo. Ta có điện áp IN1 cũng chính

4

là điện áp V-, V-<V+ nên ngõ ra OUT1 của opam =1. Qua cổng đệm không đảo
74HC245 thì T1=5V làm led D10 sáng, tín hiệu T1 này được đưa đến board MCU để
xử lý.
1.4. Board công suất 2 DC Motor L298:
a. Hình dáng:


Hình 1 - 8: Hình dáng board công suất 2 DC Motor L298
b. Sơ đồ nguyên lý:






5



Hình 1 - 9: Nguyên lý board công suất 2 DC Motor L298
c. Nguyên lý hoạt động:
Board công suất 2 DC motor gồm các khối:
 Khối ổn áp nguồn: gồm nguồn 5V cấp cho các mạch logic, 6V cấp cho RC Servo
Motor.
 Khối mạch logic : gồm các cổng NOT và NAND kết hợp nhau nhằm tạo ra các
ngõ vào gồm tín hiệu PWM ( tốc độ), DIR (đảo chiều quay) và các ngõ ra IN1,
IN2 tương thích với logic điều khiển của L298 . Bảng trạng thái tín hiệu điều
khiển 1 kênh DC Motor như hình sau:

Hình 1 - 10: Bảng trạng thái tín hiệu điều khiển 1 kênh DC Motor

Trong bảng trên ta thấy: để Motor có thể quay thuận, quay ngược và thay đổi tốc độ ta
cần 2 kênh điều chế xung PWM, điều này lại hạn chế đối với vi điều khiển hỗ trợ ít
kênh PWM. Để giải quyết điều này thì khối logic đã đảm nhiệm, với ngõ vào PWM và
DIR ta dễ dàng điều khiển DC Motor, lúc này ta chỉ cần một kênh PWM để thay đổi tốc
độ và 1 chân tín hiệu số để đảo chiều quay.

6


1.5. Boar vi điều khiển PIC16F887:
a. Hình dáng:

Hình 1 - 11: Hình dáng board vi điều khiển
b. Sơ đồ nguyên lý:
Khối nguồn vào



Khối báo yếu nguồn:


7

Khối vi điều khiển:

Các kết nối:



Các chức năng phụ:

Hình 1 - 12: Sơ đồ nguyên lý board vi điều khiển PIC16F887 cho xe MCR.


8

c. Nguyên lý hoạt động:
 Khối nguồn: nguồn vào từ pin Lipo 11.1V, có led báo nguồn, zenner 12V hạn
quá áp. Ổn áp 5V dùng LM2576, dòng 3A, nguồn này sử dụng cấp cho board vi
điều khiển và board cảm biến.
 Khối báo yếu pin: việc sử dụng pin Lipo cấp cho xe có nhiều ưu điểm nổi trội:
dung lượng cao 1500mA ( thời gian sử dụng xe lâu hơn), khối lượng nhẹ; nhưng
nó cũng có những hạn chế nhất định: giá thành cao, nếu sử dụng không đúng qui
cách ( sử dụng dưới áp giới hạn của pin) thì tuổi thọ của pin sẽ giảm đáng kể,
thậm chí sẽ bị hỏng. Do đó, mạch báo yếu pin trong quá trình sử dụng xe là hết
sức cần thiết. Ngưỡng điện áp so sánh yếu pin đã được chỉnh trước là 10VDC,
nếu điện áp pin dưới ngưỡng này thì đèn đỏ sẽ sáng lên và pin phải cần đem đi
sạc.
 Khối vi điều khiển: MCU sử dụng là PIC 16F887, dao động xung clock 20Mhz.
 Các cổng kết nối: ICSP dùng cho nạp chíp, SENSOR dùng cho kết nối với board
xử lý sensor, DC MOTOR dùng kết nối board công suất.
 Các chức năng phụ: BUTA, BUTB, BUTC sử dụng cho các chức năng như
START, MODE….; còi BUZZER dùng cho gỡ rối chương trình; LEDA, LEDB là
các user led dùng chung với DIPSW4 để hiển thị các trạng thái test xe; DIPSW4
sử dụng mục đích test các chức năng trên xe.

9

CHƢƠNG 2: ĐIỀU KHIỂN MODULE CỦA XE TDC MCR
2.1. Khối LED
a. Hình nguyên lý của LED:

Hình 2- 1: Nguyên lý Led
b. Các loại Led thông dụng

Hình 2- 2: Các loại led thông dụng
c. Led thực tế trên board

Hình 2- 3: 2 Led trên board
d. Điều khiển Led
Led (diốt phát quang) thường dùng trên các mạch điện tử để hiển thị thông tin, với 2
trạng thái tắt/sáng. Led thường được mắc nối tiếp với một điện trở (có giá trị khoảng từ
100Ω đến 2kΩ) để hạn dòng (tránh làm hỏng led), thành một đoaạn mạch AB nối tiếp.

10

Khi đó để làm led sáng, ta đặt điện thế +5V vào đầu A và điện áp 0V vào đầu B còn lại.
Xem hình 2-4

Hình 2- 4: Sơ đồ nguyên lý khối Led
Để điều khiển led bằng VĐK ta cũng áp dụng tương tự. Đầu A ta nối vào 1 chân vi điều
khiển (LEDA nối vào chân RC5, LEDB nối vào chân RD7), đầu B ta nối GND (0V). Khi
RC5 = 1 (5V) thì LEDA sáng, và khi RC5 = 0 (0V) LEDA tắt (tương tự LEDB).
2.2. Khối Loa Beep
a. Hình thực tế trên board

Hình 2- 5: Buzzer thực tế trên board
b. Sơ đồ nguyên lý khối loa beep

Hình 2- 6: Sơ đồ nguyên lý điều khiển Buzzer

11

Khối Loa beep tắt/mở bằng một khóa điện tử và được điều khiển bởi chân RD6 của
VĐK. Khi RD6 = 1 (+5V): OFF, loa không phát ra tiếng kêu; khi RD6 = 0 (0V): ON, loa
phát ra tiếng kêu.
2.3. Khối DipSwitch
a. Hình dipswitch thực tế

Hình 2- 7: Dipswitch thực tế
b. Sơ đồ nguyên lý dipswitch

Hình 2- 8: Sơ đồ nguyên lý dipswitch 4
c. Dipswitch 4 thực tế trên board

Hình 2- 9: Dipswitch 4 trên board
d. Điều khiển dipswitch
Dipsw gồm nhiều công tắc hoạt động độc lập nhưng được gắn chung với nhau thành
một thanh. Có nhiều loại Dipsw, khác biệt lớn nhất giữa chúng là số lượng công tắc.
Trong mạch VĐK dùng Dipsw-4, tức là Dipsw có 4 công tắc.
Về nguyên lý thì rất đơn giản, khi gạt một công tắc lên ON thì hai chân tương ứng với
hai phía công tắc đó sẽ nối với nhau.

12

 SW0 nối với chân RB1 của VĐK
 SW1 nối với chân RB2 của VĐK
 SW2 nối với chân RB4 của VĐK

 SW3 nối với chân RB5 của VĐK
Khi các SW ở vị trí OFF các chân RB1, RB2, RB4, RB5 = 1(+5V), các SW tương ứng ở
vị trí ON thì các chân tương ứng của VĐK = 0(0V). SW0 ở vị trí ON thì RB1 = 0(0V).
2.4. Khối nút nhấn.
Mỗi nút nhấn có hai chân, nguyên lý hoạt động rất đơn giản, khi không nhấn nút (OFF)
thì hai chân của nút không nối nhau, và ngược lại khi nhấn nút (ON), hai chân của nút
sẽ nối nhau.
a. Sơ đồ nguyên lý khối nút nhấn

Hình 2- 10: Sơ đồ nguyên lý khối nút nhấn
b. Hình thực tế khối nút nhấn trên board

Hình 2- 11: Nút nhấn thực tế trên board
Trên mạch VĐK có 3 nút nhấn được kết nối theo sơ đồ, BUTA nối với chân RB3, BUTB
nối với chân RB6, BUTC nối với chân RB7 của VĐK. Đồng thời chân của các nút nhấn
cũng được nối với chân INT (chân tạo sự kiện ngắt) của VĐK (chân này dùng để báo

13

cho VĐK biết có nút nhấn vừa mới nhấn, tạm dừng các chương trình đang chạy để
xem rằng nút nào được nhấn từ đó xử lý. Sau quá trình xử lý chương trình tiếp tục
chạy tại vị trí bị dừng trước đó).
Khi không nhấn (OFF) các chân nối với nút nhấn của VĐK sẽ nhận được mức 1 (+5V),
và khi nút nhấn được nhấn (ON) ta nhận được mức 0 (0V).
c. Điều khiển khối nút nhấn
 Chống rung phím nhấn
Vấn đề rung, nhiễu khi nhấn nút.


Hình 2- 12: Tín hiệu rung nhiễu khi nhấn nút

Khi không nhấn nút tín hiệu ở mức cao, khi nhấn nút tín hiệu xuống mức thấp. Trong
khoảng thời gian tín hiệu chuyển từ mức cao xuống mức thấp sẽ xảy ra tình trạng rung,
nhiễu làm cho tín hiệu không rõ ràng. Mặc dù khoảng thời gian rung, nhiễu là rất nhỏ,
chưa tới 1ms (phụ thuộc vào cách nhấn nút và chất lượng nút nhấn), nhưng với tốc độ
xử lý rất cao của VĐK thì đây là một vấn đề cần phải giải quyết.
 Giải pháp chống rung
Có hai giải pháp thường được đưa ra để lựa chọn là giải pháp phần cứng (thực hiện
trong giai đoạn thiết kế mạch), và giải pháp phần mềm (lúc lập trình). Ở đây hướng dẫn
các bạn một giải thuật chống rung nút nhấn bằng phần mềm
2.5. Khối RC Servo
RC Servo là hệ thống gồm động cơ DC, hộp số, và vi mạch điều khiển. Tùy theo tín
hiệu điều khiển mà nó nhận được, RC Servo sẽ quay trục đến một góc xác định trong
khoảng từ 0
0
đến 180
0
.
Khoảng thời gian
rung, nhiễu- Tín
hiệu không rõ ràng

14

a. Hình thực tế RC servor

Hình 2- 13: RC Servo
b. Điều khiển RC Servo
Điều khiển RC Servo chính là điều khiển tín hiệu vào dây PWM để RC Servo quay theo
góc mà mình mong muốn. Tín hiệu PWM cho RC Servo có chu kỳ 16ms, độ rộng xung
từ 0,7ms đến 2.3ms, mô tả theo hình bên dưới:

Hình 2- 14: Tín hiệu PWM điều khiển RC Servo
2.6. Khối động cơ DC
Động cơ điện một chiều (hay động cơ DC) là một động cơ hoạt động với dòng điện một
chiều
Đen GND
Đỏ +6V
Trắng PWM

15

a. Hình động cơ DC

Hình 2- 15: Động cơ DC
b. Điều khiển chiều của động cơ điện một chiều.
Khi ta cấp điện thế đương (+) vào một cực của động cơ và cấp điện thế âm (-) vào cực
còn lại thì động cơ sẽ quay theo một chiều cố định. Và khi ta cấp điện thế ngược lại,
đảo chiều dương âm thì động cơ sẽ quay theo chiều ngược lại.


Hình 2- 16: Điều khiển chiều quay động cơ điện một chiều
c. Điều khiển tốc độ động cơ điện một chiều
Để điều khiển tốc động động cơ điện một chiều ta dùng phương pháp điều xung
(PWM). Như trên ta đã biết, khi được cấp điện thì động cơ quay, và khi không cấp điện
nữa thì động cơ ngừng hoạt động. Trong một khoảng thời gian rất ngắn 128us (gọi là
chu kỳ điều xung T), và chia khoảng thời gian này thành 2 phần, ta sẽ cấp điện cho một
động cơ một phần thời gian đầu, và ngừng cấp điện ở phần thời gian sau. Lặp lại chu
kỳ đó liên tục, như thế động cơ sẽ liên tục ở trạng thái quay-ngừng-quay-ngừng-quay…
Vì chu kỳ điều xung là rất nhỏ nên ta sẽ thấy động cơ quay liên tục. Tốc độ của động
cơ phụ thuộc vào tỷ lệ thời gian động cơ được cấp điện trong một chu kỳ.

16


Hình 2- 17: PWM điều khiển tốc độ động cơ
2.7. Khối Xử lý sensor
a. Hình khối sensor




Hình 2- 18: Khối sensor

b. Tín hiệu
8 Sensor được nối với board xử lý. Tín hiệu từ board xử lý (T1-T8) được đưa về VĐK.
Ví dụ về trạng thái của sensor tương ứng với màu đen, trắng trên đường đua
Giá trị của sensor

sensor
7

6

5

4

3

2

1

0


Sensor
thứ 1
bên trái


Sensor
thứ 2
bên trái


Sensor
thứ 3
bên trái


Sensor
trung
tâm


Sensor
trung
tâm

Sensor
thứ 3
bên
phải


Sensor
thứ 2
bên
phải


Sensor
thứ 1
bên phải


White


Black


Black


Black

Black


White


White


Black



















1


0


0


0


0


1


1


0




17

CHƢƠNG 3: XÂY DỰNG GIẢI THUẬT ĐIỀU KHIỂN
3.1. Cấu trúc chƣơng trình

 Chương trình chia làm 4 trạng thái chính:










Hình 3- 1: Sơ đồ giải thuật
 Bước chuyển 1 (I  II): khi thấy nửa line bên trái.

Hình 3- 2: Sensor bắt được nửa line bên trái
 Bước chuyển 2 (II  I): khi đã đi qua được đoạn đường chuyển làn trái.
Xử lý các
đoạn đường
thẳng và
đường cong
(I)
Xử lý các
đoạn đường
cua vuông 90
0

(IV)
Xử lý đoạn
đường chuyển
làn bên trái

(II)
Xử lý đoạn
đường chuyển
làn bên phải
(III)
1
2
3
4
5
6
7
8

18


Hình 3- 3: Chạy qua đoạn chuyển làn trái
 Bước chuyển 3 (I  III): khi thấy được nửa line bên phải.

Hình 3- 4: Sensor bắt được nữa line bên phải
 Bước chuyển 4 (III  I): khi chạy qua đoạn chuyển làn phải

Hình 3- 5: Chạy qua đoạn chuyển làn trái

19

 Bước chuyển 5 (I  IV), bước chuyển 7 (II  IV), bước chuyển 8 (III  IV): ngay
khi thấy được một line.

Hình 3- 6: Sensor bắt được nguyên line
 Bước chuyển 6 (IV  I): khi chạy xong đoạn đường cua 90
0
.

Hình 3- 7: Chạy xong đoạn đường cua 90
0

3.2. Giải thuật xử lý khi qua các đoạn đƣờng thẳng và cong.
Các trạng thái sensor có thể gặp thể hiện tương đối độ lệch hướng của xe với phương
đường đua:
Ta dựa vào trạng thái sensor để biết được độ lệch hướng chạy của xe so với phương
của đường đua, ứng với một trạng thái sensor ta chọn một góc bẻ hợp lý sao cho xe có
xu hướng di chuyển về phía chính giữa đường đua, với độ lệch hướng càng lớn thì ta
chọn góc bẻ lái càng lớn.