4. Võ Đại Mau 171
6.3. Sơ đầu nguyên lý hệ thống điện và các module chức năng
Nguồn điều khiển gồm 1 pin 18650 thông qua mạch tăng ápXL6009 để c p ấ
nguồn 5 V cho vi điều khiển VDD,VSS và c m bi n ả ế VCC,GND. Riêng Encoder
thì được cấp nguồn 3,3 V b ng mằ ạch AMS1117thông qua JP2.
Nguồn động lực gồm 4 pin 18650 cấp cho động cơ DC 12V, RC 6V , mạch cách ly và driver TB6612 5V thông qua 3 m ch gi m áp ạ ả LM2596.
Khối c m bi n: ả ế
Tín hiệu đọc t c m bi n s ừ ả ế ẽ đưa tới Slave 1 để x lý thông qua 7 chân ử RA0RA3
và RE0RE2.
Câp nguồn điều khi n thông qua JP1. ể
Vi điều khiển:
Cả 3 vi điều khi n nh n ngu n qua JP1. ể ậ ồ
Master và 2 Slave s giao ti p thông qua chu n SPI ẽ ế ẩ JP JP9, 10. Chân RA1để
chọn Slave 1, RA2 đểchọn Slave 2.
Slave 2 điều khiển động cơ DC và RC lần lượt qua chân RC0 và RC1 b ng ằ
xung PWM thông qua m ch cách ly b ng opto ạ ằ PC817 (GU1cho động cơ DC và
GU2 cho động cơ RC).
Driver:
Cấp ngu n ồ 5 V qua JP5 cho chân VCC và GND, k méo ức cao chân STBY và chân AIN1để cho quay cùng chiều kim đồng hồ.
Chân PWMA nh n output t mậ ừ ạch cách ly t ừJP7. Chân VM và GND nh n ngu n ậ ồ 12 V t ừJP3.
Chân AO1 và AO2đưa tín hiệu output điều khiển động cơ DC qua JP8.
Khối động cơ:
Động cơ DC cho 2 chân A và B trẻ về tín hiệu Encoder cho Slave 2 thông qua
11
CHƯƠNG 6: THIẾT KẾ HỆ THỐNG ĐIỆN CHO ROBOT
Động cơ RC được cấp ngu n ồ 6 V qua JP4 và nh n tín hiậ ệu điều khi n qua ể
chân PWM t mừ ạch cách ly JP7.
Khối m ch cách ly nguạ ồn:
Mạch cách ly ngu n ồ được dùng để cách ly nguồn động lực và nguồn điều khiển,
ởđây, dùng để sử dụng nguồn ngoài c p xung cho Driver để ấ chứ không l y tr c tiấ ự ếp nguồn 5 V t ừ vi điều khiển để ắ b n xung cho Driver, vì khi dùng động cơ sẽ có dòng
ngược tr vả ề, mặc dù Driver đã có Diver diode cản dòng nhưng để an toàn thì c n thiầ ết phải có m ch cách ly ngu n. ạ ồ
Ởđây, sử dụng 2 kênh cách ly, 1 kênh dùng cho động cơ DC, 1 kênh dùng cho động cơ RC.
Đối với kênh 1, ngõ ra PWM t chân ừ RC1 của vi điều khi n Slave 2 s c p vào ể ẽ ấ
chân IN1 c a m ch cách ly và chân ủ ạ GND s kéo ẽ GND chung v i nguớ ồn điều khiển. Chân GU1 s c p vào chân PWM cẽ ấ ủa động cơ RC, tương ứng chân U1 s c p nguẽ ấ ồn
động l c ự 6 V t ừLM2596.
Đối với kênh 2, ngõ ra PWM t ừchân RC0 của vi điều khi n Slave 2 s c p vào ể ẽ ấ
chân IN2 c a mủ ạch cách ly và chân GND s kéo ẽ GND chung v i nguớ ồn điều khi n. ể
Chân GU2 s c p vào chân PWMA c a kh i Driverẽ ấ ủ ố , tương ứng chân U2 sẽ cấp nguồn động lực 5 V t ừLM2596.
Hình 6.19. M ch nguyên lý m ch cách ly 2 kênhạ ạ
Sau khi th c nghi m thì l a ch n giá trự ệ ự ọ ịđiện tr ởR2220 , thì đầu ra điện áp PWM so với đất và điện áp 2 đầuM1 so v i ớ M2 c a kh i Driver sủ ố ẽ tuyến tính.
Hình 6.20. K t quế ả thực nghi m l a chệ ự ọn điện tr ởR2 cho m ch cách ly ạ
Hình 6.21. Khối ngu n c a mồ ủ ạch điện hệ thống 0 2 4 6 8 10 12 14 U
CHƯƠNG 6: THIẾT KẾ HỆ THỐNG ĐIỆN CHO ROBOT
Hình 6.22. Khối c m bi n c a mả ế ủ ạch điện hệ thống
Hình 6.23. Khối vi điều khiển Master của mạch điện hệ thống
Hình 6.24. Khối vi điều khiển Slave 1 của mạch điện hệ thống dùng đọc và xử lý tín hiệu từ c m biả ến
Hình 6.25. Khối vi điều khiển Slave 2 của mạch điện hệ thống dùng điều khiển động cơ
CHƯƠNG 6: THIẾT KẾ HỆ THỐNG ĐIỆN CHO ROBOT
Hình 6.28. Khối m ch cách ly ngu n c a mạ ồ ủ ạch điện hệ thống
6.4. Lưu đồ giải thuật điều khiển
6.4.1. Lưu đồ giải thuật điều khiển khối master
Nhiệm vụ
Vi điều khiển master có nhiệm vụ nhận tín hiệu từ các slave, bao g m: ồ
- Vịtrí tương đối x mm c a Robot so vủ ới đường tâm line. - Giá tr c m bi n 1 ị ả ế (CB1) và c m bi n 7 (CB7). ả ế
Từ các giá trị nhận được, master n hành th c hi n 3 tác vtiế ự ệ ụ chính: - Từ giá tr ịx nhận được, master tính toán được giá trịđiều khi n ể
cho động cơ, bao gồm v n tậ ốc cho rộng cơ RC servo và vận tốc v cho động cơ DC.
- Nhận di n và xệ ử lý giao l (B, D, A) c a các ộ ủ đường line: T sa ừ
bàn đềbài đã cho, ta nh n th y Robot s 5 lậ ấ ẽ ần đi qua các giao lộ, dựa vào các giá trị 2 c m bi n 1 và 7 tr v . ả ế ả ề Master có th nhể ận diện giao lộ và tr v các giá trả ề ị và vtương ứng.
Bảng 6.9. So sánh giá trị cảm biến đểxác định giao lộ Cảm bi n 1 ế Cảm bi n 7 ế Giao l ộ Các giá trị trả ề v 300 300 1 (điểm B) 1, v1, t1 300 300 2 (điểm C) 2, v2, t2 300 300 3(điểm A) 3, v3, t3 300 300 Không g p giao l ặ ộ , v
Với t1, t2, t3 là thời gian đểđi hết các giao lộtương ứng.
Sau quá trình tính toán, master sẽ tiến hành g i các giá tr ử ị, v cho slave 2, kết thúc một chu k l y m u. ỳ ấ ẫ
Input/Ouput
Tín hi u Input ệ :
- Khoảng cách tương đối của xe so với đường tâm line x mm . - Giá trị analog c a 2 c m bi n ngoài cùng CB1 và CB7. ủ ả ế
Tín hi u Output: ệ
- Vận tốc cho động cơ RC servo. - Vận tốc vcho động cơ DC.
Giải thích lưu đồ ả gi i thu t ậ
Tại đầu m t chu k l y m u. Master gộ ỳ ấ ẫ ửi tín hi u yêu c u giao ti p vệ ầ ế ới slave 1, sau đó slave 1 gửi ngược lại giá tr mà master yêu c u. Khi nhị ầ ận đủ các giá tr ị
x mm , CB1, CB7 thì master s xét xem có g p giao l không, b ng cách so ẽ ặ ộ ằ
sánh giá tr CB1 và CB7 v i các giá tr cho trong b ng. ị ớ ị ả Trong trường h p gợ ặp giao l , master s g i cho slave 2 các giá tr ộ ẽ ử ị và v tương ứng trong thời gian xác đinh.Ngược lại, n u không g p giao l , master s tính toán giá tr ế ặ ộ ẽ ị và v
CHƯƠNG 6: THIẾT KẾ HỆ THỐNG ĐIỆN CHO ROBOT
6.4.2. Lưu đồ giải thuật điều khiển khối slave 1
Nhiệm v ụ
Vi điều khiển slave 1 có nhiệm vụ thu, nhận và x lý tín hi u c m bi n, ử ệ ả ế
tính toán vịtrí tương đối của Robot so với đường tâm line x mm sau đó truyền về cho master.
Input/Output
Tín hi u input: ệ
- Giá trị analog c a 7 c m bi n ủ ả ế (CB1 – CB7) Tín hi u output: ệ
- Khoảng cách tương đối của Robot so với đường tâm line x mm
- Giá trị analog c a c m bi n 1 và c m bi n 7. ủ ả ế ả ế
Giải thích lưu đồ giải thuật
Khi nhận được yêu c u giao ti p t master, slave 1 tiầ ế ừ ến hành đọc giá tr ị
dãy c m bi n, tính toán kho ng cách c a Robot so vả ế ả ủ ới đường tâm line x mm .
Sau đó các giá trịx mm , CB1, CB7 cho master và ch yêu c u giao ti p tiờ ầ ế ếp theo t master. ừ
6.4.3. Lưu đồ giải thuật điều khiển khối slave 2
Nhiệm vụ
Vi điều khiển slave 2 có nhiệm v nh n giá tr ụ ậ ị và v từmaster, sau đó
tính toán và c p xung PWM ấ tương ứng cho hai động cơ, đảm bảo động cơ hoạt
động theo đúng giá trịđiều khi n t master. ể ừ
Input/Output
Tín hi u input: ệ
- Vận tốc cho động cơ động RC servo. - Vận t c ố vcho động cơDC.
Tín hi u output: ệ
- Xung PWM cấp cho động cơ RC servo. - Xung PWM cấp cho động cơ DC
Giải thích lưu đồ ả gi i thuật
Khi slave 2 nh n tích hi u ậ ệ và v từmaster, sau đó tiến hành tính toán xung PWM tương ứng để ấp cho 2 động cơ c (RC servo và DC) và ch yêu cờ ầu giao ti p ti p theo t master. ế ế ừ
CHƯƠNG 6: THIẾT KẾ HỆ THỐNG ĐIỆN CHO ROBOT
Bắt đầu
Thiết lập các thông số ban đầu
Nhận đủ giá trị x, CB1, CB7 từ Slave 1?
CB1 >700 và CB7 >700? Truyền ω1 và v cho Slave 2 t > t1?
CB1 >700 và CB7 <700? Truyền ω2 và v cho Slave 2 t > t2?
CB1 <700 và CB7>700? Truyền ω3 và v cho Slave 2 t > t3?
Tính toán giá trị ω Truyền ω và v cho Slave 2 Đ Đ S S S Đ Đ Đ S S S S
Gửi yêu cầu giao tiếp
cho Slave 1
Xe chạy hết sa bàn?
Kết thúc
S
Đ Xe vào đoạn đường cong ?
v = v1 v = v2
S
Đ Đ
Đ
Bắt đầu
Thiết lập các thông số ban đầu
Đọc giá trị các cảm biến
Tính toán khoảng cách xe so với đường tâm line (x)
Gửi giá trị x, CB1 CB7 , cho Master Nhận tín hiệu yêu cầu
giao tiếp từ Master?
Robot chạy hết sa bàn? Kết thúc S S Đ Đ
CHƯƠNG 6: THIẾT KẾ HỆ THỐNG ĐIỆN CHO ROBOT
Bắt đầu
Thiết lập các thông số ban đầu
Nhậntín hiệu yêu cầu giao tiếp từ Master?
Tính toán và cấp xung điều khiển tương ứng cho hai động cơ S Đ Robot chạy hết sa bàn? Kết thúc S Đ
CHƯƠNG 7: MÔ PHỎNG CHUYỂN ĐỘNG C A ROBOT Ủ
7.1. Kết qu mô phả ỏng chuyển động của Robot
Kết qu mô ph ng chuyả ỏ ển động c a Robot ủ sau khi được thêm hàm truyền động cơ kết hợp v i bớ ộđiều khiển động cơđược th c hi n trên ph n mự ệ ầ ềm MATLAB.
Mô ph ng robot vỏ ới thời gian l y m u ấ ẫ là t0,02 s .
7.1.1. Kết qu mô ph ng chuyả ỏ ển động của Robot chưa kèm nhiễu
Kết qu mô ph ng Robot dò line theo sa bàn v i th i gian l y m u là ả ỏ ớ ờ ấ ẫ 0,02 s
với các h s ệ ốKP 0.364 và KD 0,029. Động cơ RC servo giới hạn góc quay là
8 rad s/
và Robot chạy với tốc độ trung bình là v1m s/ và gi m t c tả ố ại các khúc cua v ềv0,5m s/ .
CHƯƠNG 7: MÔ PHỎNG CHUYỂN ĐỘNG C A ROBOT Ủ
Hình 7.2. Sai số khoảng cách c a tâm c m bi n so vủ ả ế ới đường tâm line chưa kèm nhiễu
Hình 7.4.Đáp ứng v n t c cậ ố ủa động cơ DC dẫn động bánh sau chưa kèm nhiễu
7.1.2. Kết qu mô ph ng chuyả ỏ ển động của Robot kèm nhiễu
Kết qu mô ph ng Robot dò line theo sa bàn v i th i gian l y m u là ả ỏ ớ ờ ấ ẫ 0,02 s
với các h s ệ ốKP 0.364 và KD 0,029. Động cơ RC servo giới hạn góc quay là
8 rad s/
và Robot ch y vạ ới tốc độ trung bình là v1m s/ và gi m t c t i các ả ố ạ
CHƯƠNG 7: MÔ PHỎNG CHUYỂN ĐỘNG C A ROBOT Ủ
Hình 7.5. K t qu mô hình hóa Robot dò line theo sa bàn kèm nhiế ả ễu
Hình 7.7.Đáp ứng v n t c cậ ố ủa động cơ RC lái bánh trước kèm nhiễu
CHƯƠNG 7: MÔ PHỎNG CHUYỂN ĐỘNG C A ROBOT Ủ
7.2. Nhận xét k t ế quả mô phỏng
Kết qu mô ph ng ả ỏ chuyển động của Robot trước khi có nhiễu
- Thời gian xác l p cậ ủa sai số: 0,6 1
3
t s s , khi nh n giá tr sai s xác ậ ị ố
lập là e0mm.
- Độ vọ ốt l : Vì giá tr sai s mong muị ố ốn, nên độ ọ vt l chính là sai s l n nhố ố ớ ất của hệ thống e2 27mmM30mm.
- Sai s sát l p: ố ậ e0mm.
- Động cơ DC xác lập tốc độ1m s/ k tể ừ thời điểm xu t phát trong kho ng ấ ả
thời gian x p x ấ ỉ1 s .
→ Kết quả mô phỏng th a mãn các tiêu chí thi t k bỏ ế ế ộđiều khi n. ể
Kết qu mô ph ng chuyả ỏ ển động của Robot trước khi có nhiễu
- Thời gian xác l p cậ ủa sai số, khi l y sai s xác l p là ấ ố ậ e0,05mm thì
1
0,3 3
t s s .
- Độ vọ ốt l : Vì giá tr sai s mong muị ố ốn, nên độ ọ vt l chính là sai s l n nhố ố ớ ất của hệ thống e2 27mmM30mm.
- Sai s sát l p: ố ậ e3mm.
- Động cơ DC xác lập tốc độ1m s/ k tể ừ thời điểm xu t phát trong kho ng ấ ả
thời gian x p x ấ ỉ1 s .
CHƯƠNG 8: KẾT LU N Ậ
8.1. Kết quả thực nghi m ệ
Nhóm đã hoàn thànhđồ án v i các nhi m vớ ệ ụ được phân công: - Hoàn thành vi c tìm hi u t ng quaệ ể ổ n đểđặt ra đề bài phù hợp. - Hoàn thành thi t kế ếcơ khí, điện và mô hình hóa Robot xe dò line. - Thiết l p gi i thu t và ậ ả ậ chương trình giao tiếp và điều khi n Robot dò line. ể
- Vận hành, ch y th và thu k t quạ ử ế ả thực nghi m. ệ
Robot hoàn thành việc di chuy n bám line h t sa bàn theo thể ế ứ tựđã quy định v i thớ ời gian là 16 s , v i xe sớ ẽ chạy v i tớ ốc độ cao v1m s/ ở những đoạn đường thẳng và giảm t c t i các giao lố ạ ộvà các đoạ đườn ng cong. Sai s bám line ố là e2 29,74mmlớn
hơn giá trị mong mu n ố là e 15mm.
CHƯƠNG 8: KẾT LUẬN
Hình 8.2.Đáp ứng v n t c cậ ố ủa động cơ RC lái bánh trước th c tự ế
Hình 8.4. Xe g p giao l B lặ ộ ần đầu tiên và cua lê BC
CHƯƠNG 8: KẾT LUẬN
Hình 8.6. Trái - Xe g p giao l D; Ph i: Xe gặ ộ ả ặp điểm E và ra khỏi đoạn cong
Hình 8.8. Xe gặp điểm F và vào đoạn cong