Nghiên cứu, thiết kế và chế tạo robot tự hành
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 (3.06 MB, 28 trang )
CHƯƠNG 1: TỔNG QUAN VỀ ROBOT
1.1. Khái quát chung về robot
1.1.1. Khái niệm và quá trình phát triển của robot
*Sơ lược quá trình phát triển của robot
Thuật ngữ “Robot” xuất phát từ tiếng Sec (Czech) “Robota” có nghĩa là công
việc tạp dịch trong vở kịch Rossum’s Universal Robots của Karel Capek, vào năm
1921. Trong vở kịch này, Rossum và con trai của ông ta đã chế tạo ra những chiếc
máy gần giống với con người để phục vụ con người. Có lẽ đó là một gợi ý ban đầu
cho các nhà sáng chế kỹ thuật về những cơ cấu, máy móc bắt chước các hoạt động
cơ bắp của con người.
Đầu thập kỷ 60, công ty Mỹ AMF (American Machine and Foundry
Company) quảng cáo một loại máy tự động vạn năng và gọi là “Người máy công
nghiệp” (Industrial Robot).
Ngày nay người ta đặt tên người máy công nghiệp (hay robot công nghiệp)
cho những loại thiết bị có dáng dấp và một vài chức năng như tay người được điều
khiển tự động để thực hiện một số thao tác sản xuất.
Về mặt kỹ thuật, những robot công nghiệp ngày nay, có nguồn gốc từ hai
lĩnh vực kỹ thuật ra đời sớm hơn đó là các cơ cấu điều khiển từ xa (Teleoperators)
và các máy công cụ điều khiển số (NC - Numerically Controlled machine tool).
Các cơ cấu điều khiển từ xa (hay các thiết bị kiểu chủ-tớ) đã phát triển mạnh trong
chiến tranh thế giới lần thứ hai nhằm nghiên cứu các vật liệu phóng xạ. Người thao
tác được tách biệt khỏi khu vực phóng xạ bởi một bức tường có một hoặc vài cửa
quan sát để có thể nhìn thấy được công việc bên trong. Các cơ cấu điều khiển từ xa
thay thế cho cánh tay của người thao tác; nó gồm có một bộ kẹp ở bên trong (tớ) và
hai tay cầm ở bên ngoài (chủ). Cả hai, tay cầm và bộ kẹp, được nối với nhau bằng
một cơ cấu sáu bậc tự do để tạo ra các vị trí và hướng tùy ý của tay cầm và bộ kẹp.
Cơ cấu dùng để điều khiển bộ kẹp theo chuyển động của tay cầm.
Vào khoảng năm 1949, các máy công cụ điều khiển số ra đời, nhằm đáp ứng
yêu cầu gia công các chi tiết trong ngành chế tạo máy bay. Những robot đầu tiên
thực chất là sự nối kết giữa các khâu cơ khí của cơ cấu điều khiển từ xa với khả
năng lập trình của máy công cụ điều khiển số.
Di õy chỳng ta s im qua mt s thi im lch s phỏt trin ca ngi mỏy
cụng nghip. Mt trong nhng robot cụng nghip u tiờn c ch to l robot
Versatran ca cụng ty AMF, M. Cng vo khong thi gian ny M xut hin
loi robot Unimate -1900 c dựng u tiờn trong k ngh ụtụ. Tip theo M, cỏc
nc khỏc bt u sn xut robot cụng nghip : Anh -1967, Thu in v Nht
-1968 theo bn quyn ca M; CHLB c -1971; Phỏp -1972; í-1973.Tớnh nng
lm vic ca robot ngy cng c nõng cao, nht l kh nng nhn bit v x lý.
Nm 1967 trng i hc tng hp Stanford (M) ó ch to ra mu robot hot
ng theo mụ hỡnh mt-tay, cú kh nng nhn bit v nh hng bn kp theo v
trớ vt kp nh cỏc cm bin. Nm 1974 Cụng ty M Cincinnati a ra loi robot
c iu khin bng mỏy vi tớnh, gi l robot T3 (The Tomorrow Tool : Cụng c
ca tng lai). Robot ny cú th nõng c vt cú khi lng n 40 Kg.
Cú th núi, Robot l s t hp kh nng hot ng linh hot ca cỏc c cu iu
khin t xa vi mc tri thc ngy cng phong phỳ ca h thng iu khin
theo chng trỡnh s cng nh k thut ch to cỏc b cm bin, cụng ngh lp
trỡnh v cỏc phỏt trin ca trớ khụn nhõn to, h chuyờn gia ...
Trong nhng nm sau ny, vic nõng cao tớnh nng hot ng ca robot
khụng ngng phỏt trin. Cỏc robot c trang b thờm cỏc loi cm bin khỏc nhau
nhn bit mụi trng chung quanh, cựng vi nhng thnh tu to ln trong lnh
vc Tin hc - in t ó to ra cỏc th h robot vi nhiu tớnh nng c bit. S
lng robot ngy cng gia tng, giỏ thnh ngy cng gim. Nh vy, robot cụng
nghip ó cú v trớ quan trng trong cỏc dõy chuyn sn xut hin i.
Mt vi s liu v s lng robot c sn xut mt vi nc cụng
nghip phỏt trin nh sau:
Bng 1. 1. S lng robot c sn xut mt vi nc cụng nghip phỏt trin.
Nớc S X
Nhật
Mỹ
Đức
í
Pháp
Anh
Hàn Quốc
Năm 1990
60.118
4.327
5.845
2.500
1.488
510
1.000
Năm 1994
29.756
7.634
5.125
2.408
1.197
1.086
1.200
Năm 1998
67.000
11.100
8.600
4.000
2.000
1.500
-
M l nc u tiờn phỏt minh ra robot, nhng nc phỏt trin cao nht trong
lĩnh vực nghiên cứu chế tạo và sử dụng robot lại là Nhật với hình ảnh robot Asimo
hoạt động gần như con người.
Hình 1. 1. Asimo Nhật Bản
Còn ở nước ta ngành robot học cũng phát triển mạnh mẽ đặc biệt đến năm 2007
công ty cổ phần robot TOSY ở Việt Nam robot TOPIO giống hình người
Hình 1. 2. Robot TOPIO ở Việt Nam (robot đánh bóng bàn)
1.1.2. Phân loại Robot
Robot rất phong phú đa dạng, có thể được phân loại theo các cách sau
- Phân loại theo kết cấu: Theo kết cấu của tay máy người ta phân thành robot kiểu
toạ độ Đề các, Kiểu toạ độ trụ, kiểu toạ độ cầu, kiểu toạ độ góc, robot kiểu
SCARA.
- Phân loại theo hệ thống truyền động: có các dạng truyền động phổ biến là: hệ
truyền động điện, hệ truyền động thuỷ lực, hệ truyền động khí nén.
- Phân loại theo ứng dụng: dựa vào ứng dụng của robot trong sản xuất có Robot
sơn, robot hàn, robot lắp ráp, robot chuyển phôi v.v...
- Phân loại theo cách thức và đặc trưng của phương pháp điều khiển: có robot điều
khiển hở (mạch điều khiển không có các quan hệ phản hồi), Robot điều khiển kín
(hay điều khiển servo) : sử dụng cảm biến, mạch phản hồi để tăng độ chính xác và
mức độ linh hoạt khi điều khiển.
Ngoài ra còn có thể có các cách phân loại khác tuỳ theo quan điểm và mục
đích nghiên cứu.
1.3. Robot tự hành
Robot tự hành hay robot di động (mobile robot) đượ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 và có khả năng hoàn thành công việc được giao. Môi trường
hoạt động của robot 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 làm 2 lớp:
chuyển động bằng chân (legged) và bằng bánh (wheeled). Trong lớp đầu tiên,
chuyển động có được nhờ các chân cơ khí bắt chước chuyển động của con người
và động vật. Robot loại này có thể di chuyển rất tốt trên các định hình lồi lõm,
phức tạp. Tuy nhiên, cách phối hợp các chân cũng như vấn đề giữ vững tư thế làm
công việc cực kỳ khó khăn. Lớp còn lại (di chuyển bằng bánh) tỏ ra thực tế hơn,
chúng có thể làm việc tốt trên hầu hết các địa hình do con người tạo ra. Điều khiển
robot di chuyển bằng bánh cũng đơn giản hơn nhiều đảm bảo tính ổn định cho
robot. Lớp này cụ thể chia làm 3 loại robot: Loại chuyển động bằng bánh xe (phổ
biến) (hình 1.3a, c, f), loại chuyển động bằng vũng xích (khi cần mômen phát động
lớn hay khi cần di chuyển trên vùng đầm lầy, cát và băng tuyết) ( Hình 1.3 b, g, h,
I, k), và loại hỗn hợp bánh xe và xích (ít gặp).
Hình 1. 3. Một số hình ảnh về robot tự hành
* Ứng dụng robot trong sản xuất
Từ khi mới ra đời robot được áp dụng trong nhiều lĩnh vực dưới góc
độ thay thế sức người. Nhờ vậy các dây chuyền sản xuất được tổ chức lại, năng
suất và hiệu quả sản xuất tăng lên rõ rệt.
Mục tiêu ứng dụng robot nhằm góp phần nâng cao năng suất dây
chuyền công nghệ, giảm gía thành, nâng cao chất lượng và khả năng cạnh tranh của
sản phẩm đồng thời cải thiện điều kiện lao động. Đạt được các mục tiêu trên là nhờ
vào những khả năng to lớn của robot như: làm việc không biết mệt mỏi, rất dễ dàng
chuyển nghề một cách thành thạo, chịu được phóng xạ và các môi trường làm việc
độc hại, nhiệt độ cao, “cảm thấy” được cả từ trường và “nghe” được cả siêu âm ...
Robot được dùng thay thế con người trong các trường hợp trên hoặc thực hiện các
công việc tuy không nặng nhọc nhưng đơn điệu, dễ gây mệt mỏi, nhầm lẫn.
Tiềm năng ứng dụng của robot tự hành hết sức rộng lớn. Có thể kể
đến robot vận chuyển vật liệu, hàng hóa trong các tòa nhà, nhà máy, cửa hàng, sân
bay và thư viện, robot phục vụ quét dọn đường phố, khoang chân không, robot
kiểm tra trong môi trường nguy hiểm, robot canh gác, do thám, robot khám phá
không gian, di chuyển trên hành tinh, robot hàn, sơn trong nhà máy, robot xe lăn
phục vụ người khuyết tật,robot phục vụ sinh hoạt gia đình v.v...
Ví dụ một số mô hình robot tiêu biểu ứng dụng trong sản xuất như sau:
Hình 1. 4. Mô hình robot phung thuốc sinh học
Hình 1. 5. Robot khám bệnh
Hình 1. 6. Robot nhặt chất hữu cơ làm nhiên liệu
Mặc dù nhu cầu ứng dụng cao, nhưng những hạn chế chưa giải quyết
được của robot tự hành, như chi phí chế tạo cao, đó không cho phép chúng được
sử dụng rộng rãi. Một nhược điểm khác của robot tự hành phải kể đến là còn thiếu
tính linh hoạt và thích ứng khi làm việc ở những vị trí khác nhau. Bài toán tìm
đường (navigation problem) của robot tự hành cũng không phải là loại bài toán
đơn giản như nhiều người nghĩ lúc ban đầu.
CHƯƠNG 2: THIẾT KẾ VÀ CHẾ TẠO VÀ
VIẾT CHƯƠNG TRÌNH ĐIỀU KHIỂN
CHO ROBOT TỰ HÀNH
2.1 Sơ đồ khối Robot tự hành
Nhiệm vụ các khối
- Khối nguồn: tạo ra điện áp +5V từ điện áp 12V từ acquy để cung cấp nguồn cho
khối vi điều khiển, LCD, dò đường và khối mạch giao tiếp.
- Khối vi điều khiển PIC 16F877A: có nhiệm vụ thu thập tín hiệu điều khiển từ
khối mạch dò đường, cảm biến và công tắc hành trình, sau đó xử lý tín hiệu rồi đua
ra lệnh cho các khối chấp hành để điều khiển động cơ.
- Khối mạch dò đường: bao gồm 8 bộ thu phát dùng làm đầu vào cho bộ vi xử lý để
điều khiển robot đi đúng đường.
- Khối công tắc hành trình (CTHT): dùng để giới hạn việc đi chuyển của động cơ
gắp vật và động cơ nâng vật.
- Khối nút bấm: bao gồm 3 nút bấm dùng để khởi động, lựa chọn các chiến thuật
cho Robot.
- Khối hiển thị dùng LCD: hiển thị trạng thái dò đường của Robot và hiển thị các
thông tin cần thiết của chương trình để việc giám sát chương trình được dễ dàng
hơn.
- Khối mạch giao tiếp: dùng để giao tiếp và cách ly giữa mạch điều khiển và mạch
công suất tránh nhiễu từ mạch công suất về mạch điều khiển.
- Khối mạch công suất: nhận tín hiệu từ vi điều khiển qua mạch giao tiếp để điều
khiển động cơ 2 động cơ chính và 2 động cơ nâng, gắp.
2.2 Thiết kế các khối trong mạch điều khiển Robot
2.2.1 Modul dò đường
Sơ đồ nguyên lý modul dò đường
J6
5VDC
RA0-RA1-RA2-RA3-RA5-RE0-RE1-RE2
2
1
1
2
3
4
5
6
7
8
SIL-100-02
R8
R9
R10
R11
R12
R13
R14
R15
10k
220
10k
220
10k
220
10k
220
10k
2
LED SIEU SANG DO
1
C7
LED SIEU SANG DO
CDS
2
CDS
K
CDS
2
LED SIEU SANG DO
C6
K
C5
K
CDS
2
LED SIEU SANG DO
D7
1
1
D6
C4
K
CDS
K
LED SIEU SANG DO
D5
1
C3
2
LED SIEU SANG DO
D4
1
1
CDS
K
CDS
C2
2
LED SIEU SANG DO
2
2
D3
C1
K
CDS
K
LED SIEU SANG DO
D2
1
1
D1
C16
A
R7
220
A
R6
10k
A
R4
220
A
R3
10k
A
R2
220
A
R1
10k
A
R5
220
A
D17
R43
Việc so sánh ngưỡng ADC sẽ quyết định xem đâu là vạch trắng đâu là nền sân, từ
đó quyết định đâu là mức 1 đâu là mức 0. Giá trị ADC để so sánh chỉ cần lấy lần
đầu tiên khi thử sân. Lấy mẫu ADC sân thi đấu là một phương án rất hiệu quả
trong dò đường, lấy mẫu ADC giúp tiết kiệm thời gian lấy mẫu sân so với phương
pháp dùng mạch so sánh ngày xưa. Ngoài ra lấy mẫu ADC còn rất chính xác, có
thể thích hợp với mọi loại sân thi đấu mà không sợ trường hợp ở nhà robot chạy 1
kiểu lên sân thi đấu robot lại chạy kiểu khác.
Hình ảnh Modul dò đường trong robot thực tế
2.2.2 Thiết kế khối mạch giao tiếp và mạch công suất
Sơ đồ nguyên lý mạch công suất điều khiển 2 động cơ chính
+24
+24
+12
+12
+12
+12
RL1
12V
D6
2
2
R12
E
C3
Q10
3
NPN
4
M3
1nF
220
R11
R28
2
U2
1
A
C
2
R4
3
K
PC817
100
4
2
4
3
K
PC817
100
K
C
2
A
1
K
U4
1
1N4007
LED-YELLOW
LED-YELLOW
R21
E
C1
Q4
3
NPN
M1
1nF
220
R3
1
+12
3.3k
12V
D2
D19
1N4007
2
1
D12
A
A
RL4
3.3k
MOTOR DC
MOTOR PHAI
MOTOR TRAI
R19
R20
220
220
+12
2
2
+12
1
100
2
3
K
PC817
E
R10
1
Q8
3
PNP
220
R9
R27
R17
220
3
D11
100
1
A
Q3
C
2
4
3
K
PC817
1N4739A
3.3k
E
R2
D1
1
1N4007
U1
1N4007
1
4
IRF540
K
C
LED-YELLOW
1
A
D5
2
K
R22
U3
NPN
D18
Q9
3
PNP
220
R1
IRF540
Q2
R18
D10
220
1N4739A
2
A
NPN
LED-YELLOW
Q1
3
A
Q7
3
D13
1
1
+12
MOTOR DC
3.3k
+24
Mạch giao tiếp dùng opto PC817 để khuếch đại tín hiệu điều khiển và cách
ly giữa nguồn mạch điều khiển và mạch công suất. Mạch công suất điều khiển 2
động cơ chính dùng Fet và Relay vừa tận dụng được hiệu quả của FET, vừa đơn
giản vì có relay. Mạch dùng IRF540 cho dòng tải lên đến 33A sẽ cho phép chạy
nhiều loại động cơ với công suất khác nhau, trong quá trình thi đấu có thể thay đổi
động co mà không cần phải thay đổi mạch công suất.
Do yêu cầu của mạch điều khiển 2 động cơ nâng vật và gắp vật chỉ cần đảo
chiều quay và hãm tốt nên mạch điều khiển 2 động cơ nâng vật và gắp vật có sơ
đồ nguyên lý như sau
+24
+24
+12
+12
+12
+12
12V
A
A
RL8
D3
D14
R6
Q5
3
R26
NPN
7
220
3.3k
1nF
A
C
2
4
R14
3
K
PC817
100
C2
1
R5
1
E
Q11
3
NPN
220
M2
R13
MOTOR DC
3.3k
1
100
4
E
M4
C4
MOTOR DC
2
1nF
2
C
3
K
PC817
2
A
2
U7
1
1
K
U5
1
R23
5
12V
1N4007
LED-YELLOW
2
K
LED-YELLOW
RL7
D7
D17
1N4007
NANG VAT
GAP VAT
+12
+12
+12
+12
RL10
12V
D4
1N4007
D15
A
A
RL9
6
100
2
4
3
K
PC817
E
R8
Q6
3
R25
NPN
8
220
100
+24
R7
U8
1
A
C
2
4
3
K
PC817
E
2
C
2
A
K
U6
1
R16
R15
3.3k
Q12
3
NPN
220
+24
1
R24
LED-YELLOW
1
K
LED-YELLOW
12V
D8
1N4007
D16
3.3k
+12
U9
7812
Mạch chỉ sử dụng 2 relay để đảo chiều quay và hãm có sự đơn giản và mang lại
VO
VI
1
2
GND
3
hiệu quả tối ưu nhất so với yêu cầu điều khiển đặt ra.
Hình ảnh
mạch công suất thực tế
SENSOR8
SENSOR7
SENSOR6
SENSOR5
SENSOR4
SENSOR3
SENSOR2
SENSOR1
2.3 Sơ đồ nguyên lý hoàn chỉnh của robot
RP1
RESPACK-8
U83
7805
22pX2
RV9
14%
22p
D0
D1
D2
D3
D4
D5
D6
D7
A
K
RS
RW
E
2
V SS
VDD
V EE
13
14
D0
D1
D2
D3
D4
D5
D6
D7
A
K
RS
RW
E
C29
50k
R45
B1
1
RESET1
1
10k
mode
BUTTON 2
B6
B2
BUTTON
up
BUTTON 2
B3
1
down
BUTTON 2
1000u
C55
1nF
C34 C77
1000u 1nF
-
1
3
VO
+
VI
GND
C33
JACK2
sensor E3S
CAM BIEN PHAT HIEN VAT
HT_MO
HT_GAP
HT_HA
HT_NANG
1
2
15
16
17
18
23
24
25
26
SU DUNG E3S-DS30E4
+12
+12v
DCT
PWMT
PWMP
DCP
DCG_G
DCG_M
DCN_N
DCN_H
19
20
21
22
27
28
29
30
RD0/PSP0
RD1/PSP1
RD2/PSP2
RD3/PSP3
RD4/PSP4
RD5/PSP5
RD6/PSP6
RD7/PSP7
2
1
RB0/INT
RB1
RB2
RA0/AN0
RB3/PGM
RA1/AN1
RB4
RA2/AN2/VREF-/CVREF
RB5
RA3/AN3/VREF+
RB6/PGC
RA4/T0CKI/C1OUT
RB7/PGD
RA5/AN4/SS/C2OUT
RC0/T1OSO/T1CKI
RE0/AN5/RD
RC1/T1OSI/CCP2
RE1/AN6/WR
RC2/CCP1
RE2/AN7/CS
RC3/SCK/SCL
RC4/SDI/SDA
MCLR/Vpp/THV
RC5/SDO
RC6/TX/CK
RC7/RX/DT
8
9
10
SENSOR6
SENSOR7
SENSOR8
33
34
35
36
37
38
39
40
OSC1/CLKIN
OSC2/CLKOUT
2
3
4
5
6
7
SENSOR1
SENSOR2
SENSOR3
SENSOR4
SENSOR5
D4
D5
D6
D7
RS
RW
E
CONG TAC HANH TRINH
R61
R62
1k
1k
J2
2
J4
E
RS
RW
1
JACK2 2
1
D4
D5
D6
D7
C20
C21
HT_MO
HT_MO
sensor E3S
HT_GAP
HT_HA
1nF
C22
1nF
C23
HT_NANG
1nF
HT_NANG
HT_GAP
R1
U11
C
4
A
D57
3
D58
1
2
HT_HA
E
PC817
2
A
K
J3
1k
1
K
3
1N4148
LED-YELLOW
JACK3
1nF
JACK2
PIC16F877A
+24
+24
+12
+12
+12
+12
RL1
12V
D6
2
2
R12
3
K
PC817
100
4
E
C3
Q10
3
NPN
A
C
4
R4
3
K
PC817
100
2
1
U2
1
2
4
M31nF
220
R11
R28
2
C
K
A
2
K
U4
1
1N4007
LED-YELLOW
LED-YELLOW
R21
E
C1
Q4
3
NPN
M1
1nF
220
R3
1
+12
3.3k
12V
D2
D19
1N4007
2
1
D12
A
A
RL4
3.3k
MOTOR DC
MOTOR PHAI
MOTOR TRAI
R19
R20
220
220
+12
2
2
+12
2
1
R10
E
Q8
3
R27
PNP
R17
220
3
4
R2
3
K
PC817
E
1N4007
IRF540
Q2
3
PNP
R18
220
1N4739A
2
R9
C
2
100
D11
220
Q3
A
1
D1
1
1
1N4007
3
K
PC817
100
D5
4
U1
1
C
IRF540
K
A
LED-YELLOW
1
K
R22
U3
NPN
D18
Q9
D10
220
R1
1N4739A
2
A
NPN
LED-YELLOW
Q1
3
A
Q7
3
D13
1
1
+12
MOTOR DC
3.3k
3.3k
J2
1
2
3
4
5
6
7
8
1
2
3
4
5
6
7
8
+24
+24
+12
+12
+12
SIL-100-08
+12
12V
A
A
RL8
D3
D14
Q5
3
R26
NPN
7
220
3.3k
1nF
C
4
2
A
2
R14
3
K
PC817
100
C2
1
R5
1
E
Q11
3
NPN
220
M2
R13
MOTOR DC
3.3k
1
R6
E
M4
C4
MOTOR DC
2
1nF
NANG VAT
2
100
4
3
K
PC817
U7
1
C
K
A
2
1
1
12V
1N4007
LED-YELLOW
U5
2
K
R23
5
RL7
D7
D17
1N4007
LED-YELLOW
GAP VAT
+12
+12
+12
+12
RL10
12V
A
A
RL9
D4
1N4007
D15
8
220
100
+24
R7
U8
A
C
2
4
3
K
PC817
E
2
1
R16
R15
3.3k
3.3k
+12
U9
7812
C8
C10
1nF
1000u
J1
D9
VO
VI
1
C7
1N4007
+24
C9
1nF
1000u
FU1
1
1A
3
2
JACK3
+
3
C5
1000u
Q12
3
NPN
220
+24
1
K
NPN
-
E
3
R25
-
K
PC817
Q6
+
3
R8
GND
2
4
2
C
2
A
+
100
U6
1
-
6
LED-YELLOW
1
R24
12V
D8
1N4007
D16
LED-YELLOW
K
1
U12
CRYSTAL
V SS
V DD
V EE
3
KHOI VI DIEU KHIEN
15R
1N4007
2
+
CON8
sensor E3S
J8
C28
2
1
HT_NANG
HT_HA
HT_GAP
HT_MO
LCD 16X2
1
-
J6
R6
2
LCD1
2
3
4
5
6
7
8
9
1
8
7
6
5
4
3
2
1
D447
Một vài hình ảnh về robot sau khi hoàn thiện
2.4 Chương trình điều khiển Robot
// Ten chuong trinh : dieu khien ROBOT gap vat DHSPKT Nam Dinh
// Nguoi thuc hien : vietlamddt
// Ngay thuc hien : 21/11
// Phien ban
:
// Mo ta phan cung :
// Dung PIC16F877A - thach anh 20MHz
// Cam bien do duong noi voi Port A va E,cam bien tich cuc muc 0
// Cac CTHT noi voi port B, CTHT tich cuc muc 0
//
+ Cam bien phat hien vat
noi voi pin_B0
//
+ CTHT nang tay gap
noi voi pin_B5
//
+ CTHT ha tay gap
noi voi pin_B4
//
+ CTHT gioi han mo tay gap vat noi voi pin_B2
//
+ CTHT gioi han dong gap vat
noi voi pin_B3
// Su dung LCD 16x2 de hien thi, che do 4 bit
//
+ RS
noi voi pin_D1
//
+ R/W
noi voi pin_D2
//
+ EN
noi voi pin_D0
//
+ D4
noi voi pin_D4
//
+ D5
noi voi pin_D5
//
+ D6
noi voi pin_D6
//
+ D7
noi voi pin_D7
// Port C dieu khien dong co
//
+ dao chieu dc phai
noi voi pin_C0
//
+ PWM dc phai
noi voi pin_C1
//
+ dao chieu dc trai
noi voi pin_C2
//
+ PWM dc trai
noi voi pin_C3
//
+ dao chieu dc nang, ha
noi voi pin_C4, pin_C5
//
+ dao chieu dc gap vat
noi voi pin_C6, pin_C7
//--------------------------------------// Ngay hoan thanh :
// Ngay kiem tra :
// Nguoi kiem tra :
//--------------------------------------// Chu thich
:
//=======================================
#include <16f877a.h>
#include <def_877a.h>
#device *=16 adc=8
#use delay(clock=20000000)
#fuses HS,NOPUT,NOWDT,NOPROTECT,NOLVP// thach anh tan so cao,
#include <lcd.c>// khai bao thu vien lcd 4bit
#use fast_io(b)
#use fast_io(a)
#use fast_io(e)
#use fast_io(c)
//--------------------------------------// KHAI BAO CAM BIEN, CTHT
#define e3s
rb0 //cam bien phat hien vat E3S-DS30E4
#define ht_tha
rb4 //ctht mo tay gap khi khong gap vat
#define ht_nang80 rb5 //ctht nang tay gap 80cm
#define ht_ha
rb7 //ctht ha vat
#define ht_nang
rb6 //bo thu phat quang de ha tay9, nang 20cm, 40cm
#define start
rb3 //nut an start
#define chon_san rb1 //nut an chon san trai, phai
#define cam_bien rb2 //nut an chon san trai, phai
//--------------------------------------// KHAI BAO CHAN DIEU KHIEN DONG CO
#define tha_qua rc4
#define gap_qua rc5
#define ha_qua rc6
#define nang_qua rc7
//--------------------------------------//khai bao bien chon san
#define san_phai 1
#define san_trai 2
//--------------------------------------int a,i,line,c,cambien,sovach,line_status,box,san;
char hienthi[8];// mang cam bien
void left_motor_stop();
void right_motor_stop();
void kiemtra();
void test_adc();
void lechduong();
void quaytrai();
void quayphai();
void quaytrai1();
void do_line();
void speed(signed int b0,signed int b1);
void right_motor_foward(int val);
void left_motor_foward(int val);
void left_motor_reverse(int val);
void right_motor_reverse(int val);
void speed(signed int b0,signed int b1);
//---------------------------------------
void kiemtra()
{
if(e3s==0)
//kiem tra cam bien phat hien vat
{
delay_ms(2);
if(e3s==0)
{
box++;
//bien dem qua
setup_adc(adc_off);
lcd_gotoxy(10,1);
printf(lcd_putc,"CB:%u ",box);
delay_us(20);
left_motor_stop(); right_motor_stop(); //dung xe
portc=0xff;
speed(-85,-85);
//ham de giam quan tinh cua xe
delay_ms(55);
left_motor_stop(); right_motor_stop();
//-----------------------------------------------if(box==1||box==3)
//qua don va qua doi
{
tha_qua=0;gap_qua=1;
//gap qua
lcd_gotoxy(1,2);
printf(lcd_putc,"gap qua ");
delay_ms(1000);
//delay de gap qua chat
tha_qua=1;gap_qua=1;
ha_qua=0;nang_qua=1;
//nang qua
while (sovach!=1)
{
setup_adc(adc_clock_internal);// thoi gian lay mau 2-6us
setup_adc_ports(all_analog);// chon tat ca cac dau vao tu 0-7
test_adc();
do_line();
if(cambien>=4)
{
while(cambien>=4){speed(100,100);test_adc();}
sovach++;
lcd_gotoxy(10,2);
printf(lcd_putc,"Vach:%u "sovach);
}
}
speed(70,70);
//cho xe di them 1 doan
delay_ms(650);
ha_qua=1;nang_qua=1;
//dung nang qua
if (san==san_phai) {quayphai();}
else quaytrai();
left_motor_stop(); right_motor_stop();delay_ms(50);
sovach=0;
}
//-----------------------------------------------if(box==2||box==4)
//o tha qua 1 va 2
{
left_motor_stop();
right_motor_stop(); //dung xe
speed(-75,-75);
//ham de giam quan tinh cua xe
delay_ms(45);
left_motor_stop();
right_motor_stop();
tha_qua=1;gap_qua=0; //tha qua
while(ht_tha==1)
//kiem tra ctht tha vat
{
lcd_gotoxy(1,2);
printf(lcd_putc,"tha qua ");
tha_qua=1;gap_qua=0;
}
tha_qua=1;gap_qua=1; // dung tha qua
if (box==2)
{
setup_adc(adc_clock_internal);// thoi gian lay mau 2-6us
setup_adc_ports(all_analog); // chon tat ca cac dau vao tu 0-7
quaytrai1();
//quay dau xe
}
if (box==4)
{
ha_qua=0;nang_qua=1;
//nang vat them de quay dau
lcd_gotoxy(1,2);
printf(lcd_putc,"nang qua");
delay_ms(1500);
ha_qua=1;nang_qua=1;
// dung nang qua
setup_adc(adc_clock_internal);// thoi gian lay mau 2-6us
setup_adc_ports(all_analog); // chon tat ca cac dau vao tu 0-7
quaytrai1();// quay dau xe
left_motor_stop();
right_motor_stop(); //dung xe
delay_ms(100);
lcd_gotoxy(10,1);
printf(lcd_putc,"CB:%u ",box); //hien thi len LCD
}
nang_qua=0;ha_qua=1;
//ha qua
while (sovach!=2)
{
setup_adc(adc_clock_internal);// thoi gian lay mau 2-6us
setup_adc_ports(all_analog); // chon tat ca cac dau vao tu 0-7
test_adc();
do_line();
if(cambien>=4)
{
while(cambien>=4)
{
speed(100,100);
test_adc();
}
sovach++;
lcd_gotoxy(10,2);
printf(lcd_putc,"Vach:%u "sovach);
}
}
nang_qua=1;ha_qua=1;
if(sovach==2)
{
speed(70,70);
//cho xe di them 1 doan
delay_ms(650);
if (san==san_phai) {quayphai();}
else quaytrai();
}
sovach=0;
}
//-----------------------------------------------if (box==5)
{
nang_qua=1; ha_qua=1;
tha_qua=0;gap_qua=1;
//gap qua
lcd_gotoxy(1,2);
printf(lcd_putc,"gap qua ");
delay_ms(700);
//delay de gap qua chat
tha_qua=1;gap_qua=1;
nang_qua=1; ha_qua=0;
delay_ms(500);
while (ht_nang80==1)
{
setup_adc(adc_clock_internal);// thoi gian lay mau 2-6us
setup_adc_ports(all_analog); // chon tat ca cac dau vao tu 0-7
test_adc();
do_line();
}
nang_qua=1; ha_qua=1;
}
//-----------------------------------------------if (box==6)
{
speed(-85,-85);
delay_ms(100);
if (san==san_phai)
{
//speed (70,70);delay_ms(500);
//speed(-85,60);delay_ms(700);speed (80,80);delay_ms(500);
setup_adc(adc_clock_internal); // thoi gian lay mau 2-6us
setup_adc_ports(all_analog); // chon tat ca cac dau vao tu 0-7
test_adc();
while(bit_test(line,2)==1) //kiem tra led so 5 de dung quay
{
test_adc();
setup_adc(adc_clock_internal); // thoi gian lay mau 2-6us
setup_adc_ports(all_analog); // chon tat ca cac dau vao tu 0-7
speed(-85,60);
}
speed (70,-60);delay_ms(30);
}
else
{speed (70,70);delay_ms(500);};
tha_qua=1;gap_qua=0; //tha qua
while(ht_tha==1)
//kiem tra ctht tha vat
{
lcd_gotoxy(1,2);
printf(lcd_putc,"tha qua ");
tha_qua=1;gap_qua=0;
}
tha_qua=1;gap_qua=1; // dung tha qua
do {left_motor_stop();right_motor_stop();}
while (start==1);
}
//------------------------------------------------
setup_adc(adc_clock_internal); // thoi gian lay mau 2-6us
setup_adc_ports(all_analog); // chon tat ca cac dau vao tu 0-7
}
}
}
//--------------------------------------void test_adc()
// kiem tra cam bien
{
delay_ms(1);
c=85;
// gia tri so sanh
cambien=0;
// so cam bien trong vach
for(i=0;i<=7;i++)
// 8 cam bien
{
set_adc_channel(i);
// doc gia tri cam bien kenh 0-7
delay_us(100);
// tao tre adc
a=read_adc();
// doc gia tri adc tung kenh
if(a>=c)
// so sanh gia tri dau vao voi
{bit_set(line,i);hienthi[i]=1;}
else
{bit_clear(line,i);hienthi[i]=0;cambien++;}
}
lcd_gotoxy(1,2);
// hienthi ci tri led do duong len LCD
for(i=0;i<=7;i++)
{
printf(lcd_putc,"%u",hienthi[i]);// hien thi muc cam bien
delay_us(100);
// tao tre cho lcd
}
}
//--------------------------------------void left_motor_foward(int val)
{
rc3=1;
setup_timer_2(T2_div_by_4,249,1); // bam xung 5k
setup_ccp1(ccp_pwm);
set_pwm1_duty(val);
}
//--------------------------------------void right_motor_foward(int val)
{
rc0=1;
setup_timer_2(t2_div_by_4,249,1);
setup_ccp2(ccp_pwm);
set_pwm2_duty(val);
}
//--------------------------------------void left_motor_reverse(int val)
{
rc3=0;
setup_timer_2(T2_div_by_4,249,1);
setup_ccp1(ccp_pwm);
set_pwm1_duty(val);
}
//--------------------------------------void right_motor_reverse(int val)
{
rc0=0;
setup_timer_2(T2_div_by_4,249,1);
setup_ccp2(ccp_pwm);
set_pwm2_duty(val);
}
//--------------------------------------void left_motor_stop()
{
setup_ccp1(ccp_off);
rc2=1;
rc3=1;
}
//--------------------------------------void right_motor_stop()
{
setup_ccp2(ccp_off);
rc1=1;
rc0=1;
}
//--------------------------------------//ham dieu chinh toc do 2 dong co
void speed(signed int b0,signed int b1)
{
int c0=0,c1=0;
if(b0>=0)
{
c0=1-b0*2.5;
left_motor_foward(c0);
}
else
{
c0=1+b0*2.5;
left_motor_reverse(c0);
}
if(b1>=0)
{
c1=1-b1*2.5;
right_motor_foward(c1);
}
else
{
c1=1+b1*2.5;
right_motor_reverse(c1);
}
}
//--------------------------------------//chuong trinh dieu chinh robot khi di lech duong
void lechduong()
{
if(line_status==0)
{left_motor_stop();right_motor_stop();}
if(line_status==1) speed(100,10);
if(line_status==2) speed(10,100);
}
//--------------------------------------//chuong trinh quay trai
void quaytrai()
{
lcd_gotoxy(1,2);
printf(lcd_putc,"quaytrai");
test_adc();
speed(-80,80);delay_ms(800); //cho xe lech khoi vi tri ban dau
test_adc();
while(bit_test(line,6)==1) //kiem tra led so 5 de dung quay
{
speed(-80,80);
test_adc();
}
left_motor_stop();right_motor_stop();
speed(80,-80);
//ham de giam quan tinh cua xe
delay_ms(100);
left_motor_stop();
right_motor_stop();
}
//--------------------------------------void quaytrai1()
{
lcd_gotoxy(1,2);
printf(lcd_putc,"quay dau");
test_adc();
speed(-80,80);delay_ms(800); //quay trai lan 1
test_adc();
while(bit_test(line,6)==1)
{
speed(-80,80);
test_adc();
}
speed(-80,80);delay_ms(800); //quay trai lan 2
test_adc();
while(bit_test(line,6)==1)
{
speed(-80,80);
test_adc();
}
left_motor_stop();right_motor_stop();
speed(80,-80);
//ham de giam quan tinh cua xe
delay_ms(100);
left_motor_stop();
right_motor_stop();
}
//--------------------------------------void quayphai()
{
lcd_gotoxy(1,2);
printf(lcd_putc,"quayphai");
test_adc();
speed(83,-78);
//cho xe lech khoi vi tri ban dau
delay_ms(800);
test_adc();
while(bit_test(line,2)==1)
{
speed(83,-78);
test_adc();
}
left_motor_stop();
right_motor_stop();
speed(-85,85);
//ham de giam quan tinh cua xe
