ROBOT DI DỘNG BÁM THEO VẬT
Bạn đang xem bản rút gọn của tài liệu. Xem và tải ngay bản đầy đủ của tài liệu tại đây (1.77 MB, 95 trang )
ĐỒ ÁN 2
Robot di động bám theo vật
Trong thời đại ngày nay, ngành công nghiệp đóng vai trò quan trọng
trong nền kinh tế, khoa học… những ROBOT tự động, sẽ giúp cho con người rất
nhiều, chúng đã thay thế sức con người một cách hiệu quả nhất. ROBOT được
sử dụng rộng rãi cuộc sống, nơi mà môi trường có tính độc hại , nguy hiểm cũng
như các công việc yêu cầu có độ chính xác cao. Do vậy ROBOT có tầm quan
trọng rất lớn trong thời đại ngày nay.
Là sinh viên chuyên ngành điện tử tự động, để bổ sung những kiến thức đã
học cũng như nghiên cứu những vấn đề mới trong lĩnh vực điều khiển tự động
nên chúng em quyết định chọn đề tài “ROBOT DI DỘNG BÁM THEO VẬT”.
Mặc dù đã cố gắng hoàn thành đồ án đúng thời hạn nhưng trong quá trình
thực hiện không tránh khỏi những thiếu sót. Chúng em rất mong nhận được sự
đóng góp ý kiến từ quý Thầy cô và các bạn.
GVHD: Thạc sĩ Phạm Thành Danh
1
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Robot di động bám theo vật
Chúng em xin chân thành
cảm ơn:
Thầy PHẠM THÀNH
DANH đã tận tình hướng dẫn
chúng em trong quá trình
chuẩn bị cũng như thực hiện
đề tài này.
GVHD: Thạc sĩ Phạm Thành Danh
2
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Robot di động bám theo vật
NHẬN XÉT CỦA GIÁO VIÊN HƢỚNG DẪN:
.............................................................................................................
.............................................................................................................
.............................................................................................................
.............................................................................................................
.............................................................................................................
.............................................................................................................
.............................................................................................................
.............................................................................................................
.............................................................................................................
...................................................................................................
...................................................................................................
...................................................................................................
...................................................................................................
Chữ ký giáo viên:
GVHD: Thạc sĩ Phạm Thành Danh
3
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Robot di động bám theo vật
NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN:
.............................................................................................................
.............................................................................................................
.............................................................................................................
.............................................................................................................
.............................................................................................................
.............................................................................................................
.............................................................................................................
.............................................................................................................
.............................................................................................................
...................................................................................................
...................................................................................................
...................................................................................................
...................................................................................................
...................................................................................................
...................................................................................................
...................................................................................................
Chữ ký giáo viên:
GVHD: Thạc sĩ Phạm Thành Danh
4
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Robot di động bám theo vật
MỤC LỤC
PHẦN
Trang
Phần 1.Lý thuyết định vị robot
6
Phần 2.Thiết kế mạch điều khiển
9
Phần 3. Thiết kế phần cơ Robot
18
Phần 4. Điều khiển Robot
20
Phần 5. Xử lý ảnh
25
Phần 6.Tổng quan về OPENCV
30
Phần 7. Truyền dữ liệu bằng module RF
40
Phần 8. Chương trình điều khiển
46
GVHD: Thạc sĩ Phạm Thành Danh
5
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Robot di động bám theo vật
Phần 1: LÝ THUYẾT VỀ ĐỊNH VỊ ROBOT
1.1.
Định vị cho robot:
Có 2 cách: định vị tương đối và tuyệt đối
1.1.1. Định vị tƣơng đối
Là một phương pháp phổ biến dùng để xác định vị trí tương đối của
robot di động trong mặt phẳng sau một quá trình duy chuyển nào đó so với
vị trí ban đầu. Việc giải bài toán định vị tương đối thực ra là giải bài toán
động học cho robot di động, nhằm xác định vị trí và hướng tức thời của
robot sau một quá trình chuyển động trong gốc tọa độ với bề mặt sàn của
robot ở thời điểm ban đầu.
Việc khó khăn là robot liên kết với sàn thi đấu bằng bánh xe và lớp
cao su, vì vậy việc chuyển động là nhờ vào lực ma sát, do đó việc chính xác
không cao lắm vì phục thuộc tương đối vào hai bánh xe, sự trượt
của bánh.....Sai số hệ thống là do những cơ cấu chấp hành,những thiết kế
không chính xác như: hai bánh xe không bằng nhau,không cân bằng…
Phương pháp định vị tương đối chủ yếu dựa vào số vòng quay của
motor, để xác định vị trí tương đối của robot sau một quá trình chuyển
động. Phương pháp này thì đơn giản, nhưng có khuyết điểm lớn nhất là bán
kính sai số lớn và là sai số tích lũy.
Sau đây là 1 ví dụ đơn giản:
GVHD: Thạc sĩ Phạm Thành Danh
6
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Robot di động bám theo vật
Hình 1.1: Mô hình robot đơn giản
Trong hình hai bánh xe được truyền động bằng hai motor giảm tốc
riêng biệt và có hai encorder hồi tiếp xung cho mỗi motor. Đặt vấn đề là bài
toán yêu cầu cho robot chạy một đoạn thẳng 10 cm thì có sai số không và
sai số là bao nhiêu. Để giải quyết bài toán trên ta giả sử dùng 2 encoder cho
hai bánh xe với độ phân giải là 100 xung/vòng quay và encoder được gắn
trên bánh xe có đường kính 5cm,
Gọi D là đường kính của bánh xe Encorder
S là chu vi của bánh xe encorder
P là độ phân giải của encoder
D=50mm S=2*3.14*25= 157 mm (1 vòng quay)
P=100 1góc =3.60 và 1xung=1.57mm
100 xung=64 xung
GVHD: Thạc sĩ Phạm Thành Danh
7
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Robot di động bám theo vật
Yêu cầu bài toán là cho robot chạy 1 đoạn 10cm(100mm) robot
phải chạy sao cho bánh xe encoder phải quay hết 64 xung
Trên đây chỉ là tính toán tuyệt đối và encorder xoay trong điều kiện
lý tưởng,nhưng thực tế thì thường có sai số vì:
Bánh xe không tròn
Đường kính bánh xe thực tế không bằng với bánh xe dùng
trong tính toán
Bánh xe lệch trục
Vị trí lắp bánh xe không thích hợp dẫn đến bánh xe có thể
không tiếp xúc nền
Tần số lấy mẫu của encoder
Mặt sàn không cân bằng
Trượt bánh xe so với nền
Khi quay quá nhanh làm bánh xe bị trượt một khoảng
1.1.2. Định vị tuyệt đối:
Định vị tuyệt đối là phương pháp xác định vị trí chính xác của robot,
phương pháp này được thực hiện không thể thiếu các cảm biến, encoder,
công tắc hành trình ... những phần tử này tạo cơ sở cho robot nhận biết đang
ở đâu, toạ độ bao nhiêu. Điểm khác so với định vị tương đối là:
Các phần thiết kế cơ khí không cần tuyệt đối chính xác
Luôn luôn đọc giá trị hiện tại
Điều khiển theo cơ sở logic
a. Ưu điểm của phương pháp định vị tuyệt đối:
Định vị chính xác
Vị trí sai số nhỏ,không đáng kể
GVHD: Thạc sĩ Phạm Thành Danh
8
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Robot di động bám theo vật
Cơ khí không cần thiết kế tuyệt đối chính xác
b. Nhược điểm của phương pháp này:
Tốn kém kinh phí đầu tư
Mạch thiết kế phức tạp, để đáp ứng yêu cầu điều khiển
Cần cập nhật giá trị cảm biến liên tục
Phần 2: THIẾT KẾ PHẦN MẠCH ĐIỀU KHIỂN
2.1.
Tìm hiểu encoder:
Hình 2.1 Mô hình Encoder và encoder đang sử dụng E6B2
GVHD: Thạc sĩ Phạm Thành Danh
9
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Robot di động bám theo vật
Ngõ ra của encoder có thể được xem như kênh A và kênh B như hình
dưới.
HI
HI
3
1
A
HI
ISO1
2
3
R11 170
2
7414
1
CLR
L1A
1
R1 4.7K
PRE
4
2
4
R3 4.7K
R7 130
CLK
D
Q
6
D1
7474
Q1
2N 2222
R2 14K
Q3
2N3906
0
HI
0
DIR
3
1
ISO2
4
2
R6 4.7K
L2A
1
B
2
R4 4.7K
7414
Q2
2N2222
R10 170
R5 14K
0
0
GVHD: Thạc sĩ Phạm Thành Danh
10
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Robot di động bám theo vật
Hình2.2: Trạng thái ngõ ra kênh A và kênh B
Ngõ ra của kênh A là điện áp được khuếch đại bởi transisstor Q1 và
tương thích TTL với thời gian tăng và giảm bằng mạch kích Schmittriger
L1. Hoạt động tương tự ở kênh B qua Q2 và L2. Rõ ràng là mạch kích
Schmitt kênh A tạo ra xung clock để chốt và mạch kích Schmitt kênh B nối
với ngõ vào diode của chốt. Với cấu hình này, trạng thái của ngõ ra Q của
chốt sẽ cho biết chiều quay của đĩa mã hoá.
Giả sử trạng thái logic của ngõ ra Q của SN7474 là mức cao. Thế thì
kênh B được ưu tiên dẫn trước kênh A. Nói cách khác, khi chốt được kích
cạnh, có 1 tín hiệu logic ở ngõ vào diode của nó. Dưới các điều kiện này,
bảng sự thật của mạch chốt sẽ biểu diễn ngõ ra Q của nó sẽ lên mức cao.
Chiều quay ngược chiều kim đồng hồ tồn tại dưới điều kiện kênh B dẫn
trước kênh A. Các điều kiện ngược lại làm quay theo chiều kim đồng hồ và
ngõ raQ của mạch chốt ở mức thấp.
Mạch điều khiển động cơ:
ROLE
24V
4
12V
12V
D7
1
2.2.
3
5
8
R6
2.2k
12V
6
Q3
IRF540
Q5
12V
D6
PWM1
1
4
2
3
Q4
Q2
DKRL1
330 - 1W
2
R5
2.2k
MOTOR DC
7
1
2
D8
RL1
Q7
4
1
12V
10K
3
2
RL1
10K
GVHD: Thạc sĩ Phạm Thành Danh
11
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Robot di động bám theo vật
Hình 2.3: Mạch điều khiển động cơ
Giản đồ trạng thái điện áp:
Hình 2.4: Giản đồ trạng thái điện áp ngõ ra
Nếu cho động cơ chạy thì chân 1 Opto(PWM1) phải ở mức logic
1,lúc này nội trở hai chân 3 và 4 của Opto sẽ thấp, nguồn 12v qua R5(do
Opto) dẫn nên tại chân B của Q2 sẽ đạt mức logic thấp Q2 tắt Q3 dẫn
bão hoà Q4 tắt Q5 dẫn Động cơ chạy.
Ngược lại, khi Opto tắt Q2 dẫn bão hòa Q3 tắt Q4 dẫn(nếu
có điện áp rơi trên chân E của Q4) Q5 tắt Động cơ tắt
Khi muốn đảo chiều động cơ ta chỉ cần xuất mức logic 0 tại chân
DKRL1, lúc này role 2 sẽ ON, nhờ vào những tiếp điểm của role ta sẽ đảo
chiều cấp nguồn của động cơ Động cơ chạy ngược chiều
2.3.
Mạch Detect màu trên sân thi đấu:
GVHD: Thạc sĩ Phạm Thành Danh
12
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Robot di động bám theo vật
5V
100k
4
R3
240
10K
3
LED
220
+
1
-
LM324
11
D3
QUANG TRO
2
LED
Hình 2.5: Mạch detect màu trên sân thi đấu
Nguyên lý hoạt động: Dựa vào sự thay đổi nội trở của Photo Resistor
khi được chiếu sáng hay che tối, điều này làm thay đổi nội trở (dạng
analog) tại chân (-) của Opam.Chân (+) là áp ngưỡng so sánh cho ngõ vào
(-).Vì vậy ở ngõ ra chỉ tồn tại hai trạng thái 1 nếu áp chân (-) nhỏ hơn áp tại
chân (+) và ngược lại là mức 0.
Trong sân thi đấu có hai màu, xanh và trắng, nếu led phát vào màu
trắng thì ánh sáng phản chiếu vào quang trở sẽ mạnh hơn khi đi vào màu
xanh vì vậy khi vào vạch trắng thì điện áp tại chân (-) nhỏ hơn áp tại chân
(+) ngõ ra = 1.Ngược lại khi ra khỏi vạch trắng thì áp tại chân (-) lớn
hơn áp tại chân (+) ngõ ra = 0,lấy mức logic này đưa vào mạch đảo tín
hiệu để tạo mức logic chính xác trước khi đưa vào vi điều khiển để nhận
biết màu và đưa ra điều khiển thích hợp. Ta có dạng trạng thái xung trong
hình sau:
GVHD: Thạc sĩ Phạm Thành Danh
13
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Robot di động bám theo vật
Hình 2.6: Trạng thái xung
2.4.
Mạch chọn kênh chƣơng trình:
1
R6
1
VCC
9
8
7
6
5
4
3
2
9
8
7
6
5
4
3
2
R2
U17
PRO1
PRO2
PRO3
PRO4
PRO5
PRO6
PRO7
PRO8
PRO9
PRO10
SW1
SW2
SW3
SW4
SW5
SW6
9
8
7
6
5
4
3
2
23
22
21
20
19
18
17
16
I0
I1
I2
I3
I4
I5
I6
I7
I8
I9
I10
I11
I12
I13
I14
I15
CI/O
S0
S1
S2
S3
EN
1
OUTPUT
10
11
14
13
SELECT1
SELECT2
SELECT3
SELECT4
15
4067
Dùng vi mạch 4067 chúng ta có thể chọn được 16 chương trình từ 5
chân của vi điều khiển
Bảng hoạt động như sau:
Select 4
Select 3
Select 2
Select 1
EN
Output
0
0
0
0
0
Pro_1
0
0
0
1
0
Pro_2
0
0
1
0
0
Pro_3
0
0
1
1
0
Pro_4
0
1
0
0
0
Pro_5
0
1
0
1
0
Pro_6
0
1
1
0
0
Pro_7
GVHD: Thạc sĩ Phạm Thành Danh
14
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Robot di động bám theo vật
0
1
1
1
0
Pro_8
1
0
0
0
0
Pro_9
1
0
0
1
0
Pro_10
1
0
1
0
0
SW1
1
0
1
1
0
SW2
1
1
0
0
0
SW3
1
1
0
1
0
SW4
1
1
1
0
0
SW5
1
1
1
1
0
SW6
X
X
X
X
1
No(*)
No(*) không kết nối với bất kỳ ngõ input nào,lúc này ngõ ra ở mức
1,nếu được tích cực thì ứng với mỗi trạng thái select X thì ngõ ra output sẽ
kết nối với 1 input mà ta vừa mới select X.
2.5.
Mạch nạp chƣơng trình vào vi điều khiển:
P1
U2
1
6
2
7
3
8
4
9
5
7
8
1
T2OUT
R2IN
T2IN
C1+
C2+
C3
9
RXD
10
TXD
C1V-
TO 89V51
4
MAX232
3
6
CONNECT TO PC
R2OUT
C4
C2V+
5
2
C1
C2
5V
Mạch trên dùng vi mạch Max232 dùng để chuyển từ chuẩn RS232
sang mức logic 0 ;1.Nhờ vi điều khiển có chức năng nạp trực tiếp từ max
232 nên ta đưa trực tiếp hai chân TXD,RXD từ max232 vào vi điều khiển
(Dùng phần mềm Flash Magic của Phillips để load chương trình vào,ra vi
điều khiển)
2.6.
Mạch tổng quát:
GVHD: Thạc sĩ Phạm Thành Danh
15
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Robot di động bám theo vật
A. Sơ đồ mạch Layout:
Sơ đồ mạch Capture:
GVHD: Thạc sĩ Phạm Thành Danh
16
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Robot di động bám theo vật
SW2
R5
VCC
SPARE
VCC
VCC
D8
VCC
COLOUR
3
R25
cuon day 1
R36
VCC
1
C9
104
VCC
R31
R
VCC
XTAL1
Y1
11.0592MhZ
D22
J5
C
11
LM324
C16
C
C
VCC
SW1
J4
J3
INT0
INT1
1
2
3
4
5
6
7
8
RXD
RXD
TXD
C17
C
1
2
3
SERIAL PORT
CON3
SW DIP-2
AT89C52
VCC
RL5
RL4
J1
1
2
3
ENCODER2
RL3
CON3
D21
120
R16
LED
R108
D25
220
LED
CON3
VCC
R7
1
R9
VCC
R10
VCC
R11
VCC
R12
VCC
R13
VCC
R14
VCC
R15
VCC
R17
VCC
VCC
VCC
10K
Q3
Q4
Q7
Q8
Q9
Q10
Q11
Q12
Q13
1
R6
1
Q2
4
VCC
LED
VCC
C13
1
SPEAKER
+
3
-
2
D9
R
VCC
1
R168
TXD
30
29
ALE/PROG
PSEN
EA/VPP
RST
R
RL2
R32
R
WAITING
R57
U19A
2
3
4
5
6
7
8
9
VCC
R30
R
Q6 R58 10K
LS1
100K
31
9
VCC
1
2
3
VCC
VCC
3.3K
VCC R67
RST
VCC
XTAL2
ENCODER1
VCC
XTAL1
XTAL2
C6
LED
C24
RST
R113
8.2K
19
18
RXD
TXD
INT0
INT1
OUT ENCODER1
OUT ENCODER2
RL4
RL5
R28
Q1
RL1
C30
10UF
VR100K
R106
R107
XTAL1
XTAL2
RESISTOR SIP 9
P1.0/T2
P3.0/RXD
P1.1/T2-EX P3.1/TXD
P1.2
P3.2/INT0
P1.3
P3.3/INT1
P1.4
P3.4/T0
P1.5
P3.5/T1
P1.6
P3.6/WR
P1.7
P3.7/RD
10
11
12
13
14
15
16
17
20
SW RST
C23
1
2
3
4
5
6
7
8
R4
LED
PWM5
C5
C
RL1
RL2
RL3
PWM1
PWM2
PWM3
PWM4
PWM5
2
3
4
5
6
7
8
9
9
8
7
6
5
4
3
2
PWM4
OUT
OUTPUT
SELECT1
SELECT2
SELECT3
SELECT4
SPARE
LED
WAITING
PWM3
IN
C4
C
1
2
21
22
23
24
25
26
27
28
LED
Q14
R68
P2.0/A8
P2.1/A9
P2.2/A10
P2.3/A11
P2.4/A12
P2.5/A13
P2.6/A14
P2.7/A15
GND
LED NGUON
P0.0/AD0
P0.1/AD1
P0.2/AD2
P0.3/AD3
P0.4/AD4
P0.5/AD5
P0.6/AD6
P0.7/AD7
PWM2
J15
39
38
37
36
35
34
33
32
PWM1
1
R55
DIODE
1K
POW_VXL
GND
U9 LM7805C/TO220
2
D53
LEFT3
LEFT2
LEFT1
CENTERL
CENTERR
RIGHT1
RIGHT2
RIGHT3
2
3
4
5
6
7
8
9
40
U16
R66
1
R2
R69
100K
LEFTA3
R8
120
VCC
R18
R
R19
R
R20
R
R21
R
R22
R
R23
R
R24
R
R26
R
R27
R
CENTERL1
LS6
11
1
A9
A8
A7
A10
12V
AQ5
IRL540N/TO
MOTOR AC
MG4
3
4
1
8
AD16
DIODE
AD13
AQ4
AD4
AQ22
AD1
ISO7
2
AR16
IRL540N/TO
AQ24
AR4
AQ6
AQ20
AR27 12V
AR21 12V
A6
ISO2
A9
4
LED
5
2
AQ23
ROLE
ISO1
1
D30
220
6
AR15
R
CON7
CON12
R112
AR13
R
12V
1
8
ROLE
ISO8
AR28
1
AR22
3
4
2
2
+
3
-
2
MOTOR AC
AQ21
4
U20A
LM324
MG1
3
AQ2
LS4
7
D33
6
5
2
4
A1
1
2
3
4
5
6
7
24V
7
AR3
R
1
24V
AR1
R
AQ3
SW1
SW2
SW3
SW4
SW5
SW6
LED
3
VCC
VCC
R72
100K
J13
2
LED
4
220
1
2
3
4
5
6
7
8
9
10
11
12
A4
12V
D17
PRO1
PRO2
PRO3
PRO4
PRO5
PRO6
PRO7
PRO8
PRO9
PRO10
1
4
11
14
+
12
-
CON10
D29
LED
12V
4067
J12
R111
A6
LED
LEFTA1
U19D
LM324
LED
15
VCC
13
LED
3
220
VCC
R71
100K
LED
2
8
EN
LED
4
D28
LED
1
10
R110
+
1
2
3
4
5
6
7
8
9
10
-
J7
LM324
SELECT1
SELECT2
SELECT3
SELECT4
LED
2
11
U19C
10
11
14
13
LED
1
VCC
9
S0
S1
S2
S3
LED
A5
LEFTA2
R70
100K
CI/O
OUTPUT
4
VCC
I0
I1
I2
I3
I4
I5
I6
I7
I8
I9
I10
I11
I12
I13
I14
I15
1
3
VCC
9
8
7
6
5
4
3
2
23
22
21
20
19
18
17
16
A4
LED
4
220
U17
PRO1
PRO2
PRO3
PRO4
PRO5
PRO6
PRO7
PRO8
PRO9
PRO10
SW1
SW2
SW3
SW4
SW5
SW6
A3
D27
A2
R109
A1
LM324
7
+
5
-
6
9
8
7
6
5
4
3
2
9
8
7
6
5
4
3
2
11
U19B
CENTERR1
U1056968899
AQ11
1
MOTOR AC
AR17
R
D37
R
LED
12V
1
AQ29
1
8
AD20
DIODE
AD17
ISO12
AQ12
AQ28
AR20
IRL540N/TO
3
AR8
2
3
MOTOR AC
4
ROLE
AD5
AQ8
MG5
3
AQ27
1
8
AD8
ISO3
R3
6
5
2
AQ10
LM324
LS5
7
AR19
R
4
IRL540N/TO
ROLE
2
11
24V
D40
12V
3
AQ9
U20C
-
AQ30
VCC
AQ26
ISO4
ISO10
AR24
RIGHT A2
AR30
3
2
3
2
40106
7
VCC
R75
4
OUT ENCODER1
2
AR29 12V
A10
4
1
1
A7
ENCODER1
4
AR23 12V
U18A
1
14
8
+
9
12V
6
5
2
VCC
10
LS2
7
D20
74HC540
4
U20D
11
100K
LM324
R64
D38
R
LED
VCC
14
14
+
U18B
12V
LS3
24V
4
12
-
13
C10
C
ENCODER2
3
OUT ENCODER2
4
A3
AR9
R
6
AR11
R
5
2
AQ15
7
40106
7
D24
RIGHTA3
VCC
MG3
MOTOR AC
4
ROLE
4
D19
12V
3
AQ17DIODE
1
1
VCC
R29
1
8
Q32
24V
2
VCC
24V
AR7
R
LED
RIGHTA1
R74
100K
12V
AR5
R
2
R
G1
G2
A5
1
19
RIGHT3
RIGHT2
RIGHT1
CENTERR
CENTERL
LEFT1
LEFT2
LEFT3
18
17
16
15
14
13
12
11
4
D36
Y1
Y2
Y3
Y4
Y5
Y6
Y7
Y8
1
R62
A1
A2
A3
A4
A5
A6
A7
A8
2
LM324
7
+
5
-
6
2
3
4
5
6
7
8
9
4
11
U20B
RIGHTA3
RIGHT A2
RIGHTA1
CENTERR1
CENTERL1
LEFTA1
LEFTA2
LEFTA3
A2
R73
100K
1
VCC
VCC
AD12
DIODE
R39
24V
AD9
J2
AR12
IRL540N/TO
3
2
ISO5
F2
AQ18
R38
1
2
ACQUY 1
AQ14
4
1
ISO6
ACQUY 2
12V
R
D7
R
2
J8
AR25 12V
A8
LM7812C/TO220
1
3
IN OUT
GND
AQ16
2
1
C18
C
LED
GVHD: Thạc sĩ Phạm Thành Danh
17
3
2
AR26
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Robot di động bám theo vật
Phần 3: THIẾT KẾ PHẦN CƠ CHO ROBOT
3.1.
Các hình chiếu của robot:
a. Hình chiếu đứng:
Hình 3.a: Hình chiếu đứng robot tự động
b. Hình chiếu bằng:
Hình 3.b: Hình chiếu bằng robot tự động
GVHD: Thạc sĩ Phạm Thành Danh
18
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Robot di động bám theo vật
c. Hình chiếu cạnh:
Hình 3.c: Hình chiếu cạnh robot tự động
GVHD: Thạc sĩ Phạm Thành Danh
19
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Robot di động bám theo vật
Phần 4: ĐIỀU KHIỂN ROBOT
4.1.
Vi xử lý và các ngõ vào ra:
Vi điều khiển P89V51RB2FN
16KB chương trình,5 kênh điều rộng xung,hai thanh ghi
DPTR,Watch dog timer,Brown out detect
16 CTHT tương ứng cho 16 chương trình hoặc 16 công tắc
hành trình
8 ngõ vào sensor dò đường
2 ngõ vào encoder, 2 ngõ vào ngắt ngoài
5 motor điều rộng xung
2 SW tổ hợp chọn chế độ hoạt động,1 nút chọn màu của đội
đỏ/xanh
4.2.
Lập trình cho robot:
4.2.1 Đọc tín hiệu từ máy tính:
Phân vùng cảm biến : 8 led được chia thành 16 vùng (8 vùng trái và 8
vùng phải)
L3
L2
L1
CL
CR
R1
R2
R3
Với:
L3 sensor trái thứ 3 so với sensor trung tâm
L2 sensor trái thứ 2 so với sensor trung tâm
L1 sensor trái thứ 1so với sensor trung tâm
CL sensor trung tâm bên trái
CR sensor trung tâm bên phải
GVHD: Thạc sĩ Phạm Thành Danh
20
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Robot di động bám theo vật
R1 sensor phải thứ 1 so với sensor trung tâm
R2 sensor phải thứ 2 so với sensor trung tâm
R3 sensor phải thứ 3 so với sensor trung tâm
Đọc cảm biến theo nguyên tắc : đọc từ trong ra ngoài, tức là kiểm tra
giá trị sensor từ CL,CR L1,R1 L2,R2 L3,R3 Vùng error
Vùng Error sẽ được thực thi khi và chỉ khi robot đang chạy dò đường
nhưng bất ngờ không có sensor nào tích cực đã chạy sai vạch tăng
tốc bánh bên trái nếu trước đó rẽ phải hoặc tăng tốc bánh bên phải nếu
trước đó rẽ trái
A. Bảng phân vùng :
L3
L2
L1
CL
CR
R1
R2
R3
1
1
1
0
0
1
1
1
1
1
1
1
1
1
1
1
0
0
1
0
0
0
1
0
0
1
1
1
1
1
1
1
1
1
1
1
1
1
1
1
1
1
1
1
1
1
1
1
0
0
1
1
1
1
1
1
0
1
1
1
1
1
1
1
1
1
1
1
1
1
1
1
1
1
1
1
1
1
1
1
1
0
0
1
1
1
1
1
0
0
0
1
1
1
1
1
0
0
1
1
1
1
1
1
1
1
1
1
1
1
0
0
1
1
1
1
00H
0
1
1
1
1
1
1
1
1
1
01H
1
0
1
1
0
1
GVHD: Thạc sĩ Phạm Thành Danh
21
Phân vùng
Vùng trung tâm
(Clr 00,01H)
Vùng trái 1
Vùng trái 2
Vùng trái 3
Vùng trái 4
Vùng trái 5
Vùng trái 6
(Setb 00H,Clr 01H)
Vùng trái 7
Vùng phải 1
Vùng phải 2
Vùng phải 3
Vùng phải 4
Vùng phải 5
Vùng phải 6
(Setb 01H,Clr 01H)
Vùng phải 7
XXX
Vùng lạc
Lạc phải
Lạc trái
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Robot di động bám theo vật
(XXX) nếu không có sensor nào tích cực thì phải xét thêm hai bit 00H,01H
Ứng với mỗi vùng có 1 tốc độ riêng biệt robot không thể lạc vạch được
B: Bảng tốc độ: ta có 4 hàm tốc độ ứng vời mỗi hàm có 16 tốc độ
khác nhau
Vùng
Vùng trung tâm
Vùng trái 1
Vùng trái 2
Vùng trái 3
Vùng trái 4
Vùng trái 5
Vùng trái 6
Vùng trái 7
Vùng phải 1
Vùng phải 2
Vùng phải 3
Vùng phải 4
Vùng phải 5
Vùng phải 6
Vùng phải 7
Lạc trái
Lạc phải
Vùng trung tâm
Vùng trái 1
Vùng trái 2
Vùng trái 3
Vùng trái 4
Vùng trái 5
Vùng trái 6
Vùng trái 7
Vùng phải 1
Vùng phải 2
Vùng phải 3
Vùng phải 4
Vùng phải 5
Vùng phải 6
Tốc độ
Chậm
Vừa
GVHD: Thạc sĩ Phạm Thành Danh
Bánh trái
60
55
50
45
40
30
20
10
60
60
60
60
60
60
60
00
60
120
100
80
60
40
30
20
10
120
120
120
120
120
120
22
Bánh phải
60
60
60
60
60
60
60
60
55
50
45
40
30
20
10
60
00
120
120
120
120
120
120
120
120
100
80
60
40
30
20
Speed 1
Speed 1
Speed 2
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Vùng phải 7
Lạc trái
Lạc phải
Vùng trung tâm
Vùng trái 1
Vùng trái 2
Vùng trái 3
Vùng trái 4
Vùng trái 5
Vùng trái 6
Vùng trái 7
Vùng phải 1
Vùng phải 2
Vùng phải 3
Vùng phải 4
Vùng phải 5
Vùng phải 6
Vùng phải 7
Lạc trái
Lạc phải
Vùng trung tâm
Vùng trái 1
Vùng trái 2
Vùng trái 3
Vùng trái 4
Vùng trái 5
Vùng trái 6
Vùng trái 7
Vùng phải 1
Vùng phải 2
Vùng phải 3
Vùng phải 4
Vùng phải 5
Vùng phải 6
Vùng phải 7
Lạc trái
Lạc phải
Robot di động bám theo vật
Nhanh
Nhanh Nhất
GVHD: Thạc sĩ Phạm Thành Danh
120
00
120
180
140
100
80
60
40
30
20
180
180
180
180
180
180
180
00
180
255
230
200
170
140
100
50
20
255
255
255
255
255
255
255
00
200
23
10
120
00
180
180
180
180
180
180
180
180
140
100
80
60
40
30
20
180
00
255
255
255
255
255
255
255
255
230
200
170
140
100
50
20
200
00
Speed 3
Speed 4
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Robot di động bám theo vật
Giải thuật đọc tín hiệu:
GVHD: Thạc sĩ Phạm Thành Danh
24
SVTH: Nhật Duy - Đại Dương
ĐỒ ÁN 2
Robot di động bám theo vật
PHẦN 5: XỬ LÝ ẢNH
3.1.1 Các khái niệm cơ bản:
3.1.1.1 Điểm ảnh:
Ảnh trong tự nhiên là ảnh liên tục về không gian và độ sáng. Khi ảnh được
lưu vào máy tính thì ảnh đã được số hóa. Số hóa là sự biến đổi gần đúng của một
ảnh liên tục thành một tập hợp điểm phù hợp với ảnh thật về vị trí không gian và
độ sáng. Mỗi điểm như vậy gọi là một điểm ảnh ( PEL : Picture Element ) hay
còn gọi là Pixel. Điểm ảnh Pixel là là một phần tử của ảnh số tại tọa độ (x,y) với
độ sáng hoặc màu nhất định. Tập hợp những điểm ảnh tạo thành ma trận ảnh.
3.1.1.2 Độ phân giải của ảnh:
Độ phân giải ảnh là mật độ điểm ảnh được ấn định trên một ảnh số được
hiển thị. Khoảng cách các điểm ảnh được chọn sao cho mắt người thấy được sự
liên tục của ảnh và tạo nên mật độ phân bố theo hai trục x và y trong không gian
hai chiều.
3.1.1.3 Mức xám:
Mỗi điểm ảnh được đặc trưng cơ bản bởi tọa độ x, y của điểm ảnh và độ
xám. Mức xám của điểm ảnh là cường độ sáng của nó được gán giá trị số tại
điểm đó. Các thang mức xám thường dùng là: 16, 32, 64, 128, 256.Trong đó
mức 256 thường dùng nhất vì máy tính dùng 1 byte ( 2^8=256) để biểu diễn từ 0
đến 255 mức.
GVHD: Thạc sĩ Phạm Thành Danh
25
SVTH: Nhật Duy - Đại Dương
