VMHoang_Robot Tu Dong

ĐẠI HỌC QUỐC GIA THÀNH PHỐ HỒ CHÍ MINH TRƯỜNG ĐẠI HỌC BÁCH KHOA KHOA ĐIỆN – ĐIỆN TỬ BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG --------------oOo-------------

LUẬN VĂN TỐT NGHIỆP

XÂY DỰNG MÔ HÌNH MẠNG KHÔNG DÂY ĐIỀU KHIỂN HỆ THỐNG ROBOT TỰ ĐỘNG VẬN CHUYỂN HÀNG HÓA GVHD SVTH LỚP MSSV

: KS. PHAN NGUYỄN PHỤC QUỐC : VŨ MINH HOÀNG : DD08KSTD : 40800732

TP.HỒ CHÍ MINH, THÁNG 12/2012

i

BỘ GIÁO DỤC VÀ ĐÀO TẠO

CỘNG HÒA- XÃ HỘI- CHỦ NGHĨA-VIỆT NAM

TRƯỜNG ĐẠI HỌC BÁCH KHOA Thành phố Hồ Chí Minh

Độc lập – Tự do – Hạnh phúc

KHOA: ĐIỆN – ĐIỆN TỬ

-----o0o-----

NHIỆM VỤ LUẬN ÁN TỐT NGHIỆP

BỘ MÔN: ĐIỀU KHIỂN TỰ ĐỘNG Chú ý: Sinh viên phải dán tờ này vào trang thứ nhất của bản thuyết minh

HỌ VÀ TÊN: VŨ MINH HOÀNG NGÀNH: ĐIỀU KHIỂN KHIỂN TỰ ĐỘNG

MSSV: 40800732 LỚP: DD08KSTD

1-Đầu đề luận án

XÂY DỰNG MÔ HÌNH MẠNG KHÔNG DÂY ĐIỀU KHIỂN HỆ THỐNG ROBOT TỰ ĐỘNG VẬN CHUYỂN HÀNG HÓA Nhiệm vụ (yêu cầu về nội dung dung và số liệu ban đầu) 2- Nhiệm ........................................... ................................................................. ............................................ ............................................ ............................................ ........................................... ..................... ........................................... ................................................................. ............................................ ............................................ ............................................ ........................................... ..................... ........................................... ................................................................. ............................................ ............................................ ............................................ ........................................... ..................... ........................................... ................................................................. ............................................ ............................................ ............................................ ........................................... ..................... ........................................... ................................................................. ............................................ ............................................ ............................................ ........................................... ..................... Ngày giao nhiệm vụ luận án: ............................................ 3- Ngày .................................................................. ............................................ .................................... .............. Ngày hoàn thành nhiệm vụ: .......................................... 4- Ngày ................................................................ ............................................ ........................................ .................. Phần hướng dẫn 5-Họ và tên người hướng dẫn 1. ........................................... ................................................................. ......................... ... ......................................... ............................................ ... 2. ........................................... ................................................................. ......................... ... ......................................... ............................................ ... 3. ........................................... ................................................................. ......................... ... ......................................... ............................................ ... Nội dung và yêu yêu cầu LATN đã được thông qua qua Bộ môn . Ngày……tháng…….năm 200… Chủ nhiệm bộ môn (Ký và ghi rõ họ tên)

PHẦN DÀNH CHO KHOA, BỘ MÔN Người duyệt (Chấm sơ bộ): bộ): ............................................................ ............................................................................................ ............................... Đơn vị: ............................................................. Ngày bảo vệ: ............................................................... ................................................................................... .................... ................................................................................ .................... Điểm tổng kết: ............................................................ Nơi lưu trữ của luận án:

Người hướng dẫ n chính (Ký và ghi rõ họ tên)

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG

CỘNG HÒA- XÃ HỘI- CHỦ NGHĨA-VIỆT NAM Độc lập – Tự do – Hạnh phúc *** Ngày___tháng____năm_______ Ngày___tháng____năm_______

PHIẾU CHẤM BẢO VỆ LVTN (Dành cho người hướng dẫn) 1. Họ và tên SV: Chuyên ngành: 2. Đề tài:

VŨ MINH HOÀNG Điều khiển tự động

MSSV: 40800732

Xây dựng mô hình mạng không dây điều khiển hệ thống robot tự động vận chuyển hàng hóa

3. Họ và tên người hướng dẫn: KS. PHAN NGUYỄN PHỤC QUỐC 4. Tổng quát về bản thuyết minh: ................................................................. ..................................................................... .... Số trang: ........................................... Số chương: ................................. Số bảng số liệu: ............................... Số hình vẽ: ................................. Số tài liệu tham khảo: ....................... Phần mềm tính toán: .................. Sản phẩm: ......................................... 5. Tổng quát các bản vẽ: Số bản vẽ: ...................... bản bản A1 ...................... bản A2 khổ khác: ............................... Số bản vẽ tay: ...................................................... Số bản vẽ máy tính:. ................................................ 6. Những ưu điểm chính của luận án tốt nghiệp: ............................................................................................................................................................... ............................................................................................................................................................... ............................................................................................................................................................... ............................................................................................................................................................... 7. Những khuyết điểm chính của của luận án tốt nghiệp ............................................................................................................................................................... ............................................................................................................................................................... ............................................................................................................................................................... ............................................................................................................................................................... 8. Đề nghị: Được bảo vệ º Bổ sung thêm để bảo vệ º Không được bảo vệ º 9. 3 câu hỏi sinh viên viên phải trả lời trước hội hội đồng: a) ............................................................ .............................................................................................................................. .................................................................................................. ................................ ................................................................................................................................................................. b)................................................................................................................... b)................................................. ............................................................................................................. ........................................... ................................................................................................................................................................. c) ............................................................ .............................................................................................................................. .................................................................................................. ................................ ................................................................................................................................................................. 10. Đánh giá chung (bằng chữ: giỏi, chữ: giỏi, khá, trung bình)……………….Đ bình)……………….Điểm: iểm: ……./10

Kí tên (ghi rõ họ tên)

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG

CỘNG HÒA- XÃ HỘI- CHỦ NGHĨA-VIỆT NAM Độc lập – Tự do – Hạnh phúc *** Ngày___tháng____năm_______ Ngày___tháng____năm_______

PHIẾU CHẤM BẢO VỆ LVTN (Dành cho người phản biện) 1. Họ và tên SV: Chuyên ngành: 2. Đề tài:

VŨ MINH HOÀNG Điều khiển tự động

MSSV: 40800732

Xây dựng mô hình mạng không dây điều khiển hệ thống robot tự động vận chuyển chu yển hàng hóa

3. Họ và tên người hướng dẫn: KS. PHAN NGUYỄN PHỤC QUỐC 4. Tổng quát về bản thuyết minh: ................................................................. ..................................................................... .... Số trang: ........................................... Số chương: ................................. Số bảng số liệu: ............................... Số hình vẽ: ................................. Số tài liệu tham khảo: ....................... Phần mềm tính toán: .................. Sản phẩm: ......................................... 5. Tổng quát các bản vẽ: Số bản vẽ: ...................... bản bản A1 ...................... bản A2 khổ khác: ............................... Số bản vẽ tay: ...................................................... Số bản vẽ máy tính:. ................................................ 6. Những ưu điểm chính của luận án tốt nghiệp: ............................................................................................................................................................... ............................................................................................................................................................... ............................................................................................................................................................... ............................................................................................................................................................... 7. Những khuyết điểm chính của của luận án tốt nghiệp ............................................................................................................................................................... ............................................................................................................................................................... ............................................................................................................................................................... ............................................................................................................................................................... 8. Đề nghị: Được bảo vệ º

Bổ sung thêm để bảo vệ º

Không được bảo vệ º

9. 3 câu hỏi sinh viên phải trả lời trước hội đồng: a) ........................................................... ............................................................................................................................. ................................................................................................... ................................. ................................................................................................................................................................. b).................................................................................................................. b)................................................ .............................................................................................................. ............................................ ................................................................................................................................................................. c) ........................................................... ............................................................................................................................. ................................................................................................... ................................. ................................................................................................................................................................. 10. Đánh giá chung (bằng chữ: giỏi, chữ: giỏi, khá, trung bình)……………….Điểm bình)……………….Điểm:: ……./10

Kí tên (ghi rõ họ tên)

NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… …………………………………………………………………………………………………

NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… ………………………………………………………………………………………………… …………………………………………………………………………………………………

Lờ i cảm ơn

Xin đượ c bày t ỏ lòng biết ơn sâu sắc đế n những ngườ i thầ y t ận tu ỵ trên giảng đườ ng. Nhữ ng t ấm gương đã dạ y cho em nhiều điều hơn kiế n thức, đó là nhân cách và tấ m lòng.

Xin đượ c cảm ơn nhữ ng tháng ngày học t ậ p và ho ạt động dướ i mái trườ ng Bách Khoa thân yêu. Hành trang vào đờ i không chỉ có nhữ ng điề u thầy đã dạ y, mà còn mang theo cả bản lĩnh và nhiệt huyế t của tuổ i tr ẻ ta có được sau hơn 4 năm rèn luyệ n t ại trườ ng. Xin cảm ơn những ngườ i bạn chân thành tôi đã có, những ngườ i

cùng tôi vượ t qua nhiề u th ử thách, cùng chia sẻ thành công, thấ t bại, niề m vui, nỗ i buồn nhữ ng ngày qua. C ảm ơn gia đình. Điể m t ựa vữ ng chắ c nhấ t cho t ất cả nhữ ng thành công trong cuộc số ng.

Xin đượ c chân thành c ảm ơn thầ y Phan Nguyễ n Phục Quốc đã t ận tình chỉ bảo và t ạo mọi điề u kiện thuận lợi để em có th ể hoàn thành t ốt lu ận văn t ố t nghiệ p này. Xin cảm ơn câu lạc bộ nghiên c ứ u khoa h ọc khoa Điện – Điện t ử và những ngườ i bạn ở phòng TT- Lab đã giúp đỡ , ủng hộ tôi về mặt ý tưở ng luận văn cũng như về mặt tinh thần để tôi có đủ bản lĩnh và ý chí hoàn

thành quãng đườ ng học t ậ p t ại đại học Bách Khoa.

Hồ Chí Minh, Ngày 17 tháng 12 năm 2012

Vũ Minh Hoàng ii

TÓM TẮT NỘI DUNG LUẬN VĂN Luận văn này nhằ m m ục tiêu thiế t k ế và xây d ự ng m ột h ệ th ố ng robot t ự động v ận chuyể n hàng hóa có m ột tr ạm điề u khiể n trung tâm làm nhi ệm vụ phân ph ối đườ ng chạ y, c ậ p nh ật vị trí các robot slave và x ử lý các trườ ng hợp xung đột làn đườ ng. Việc lự a chọn đề tài này nhằ m các mục đích sau: M ục đích thứ nhấ t là ứ ng d ụng các ki ế n thức đã họ c vào th ự c t ế. M ục đích thứ hai là đáp ứ ng nhu c ầu t ừ thự c t ế c ần m ột hệ thố ng robot vận chuy ể n hàng hóa t ự động trong các kho hàng nh ằ m giải phóng s ức lao động của con ngườ i. Trong khuôn kh ổ một luận văn tố t nghiệp đại h ọc, ở đây ta chỉ xây d ự ng m ột mô hình h ệ thố ng thử nghiệm, nghĩa là giả i thuật chương trình trên mô hình có thể áp d ụng tr ự c ti ế p vào hệ thố ng vận hành th ật sự vớ i một số các ràng bu ộc về phần điện. Mô hình trong lu ận văn đượ c thiế t k ế gồm hai robot slave di chuy ể n trên sàn làm vi ệc theo

phương pháp dò line được điề u khiể n bởi vi điề u khiể n ARM Cortex M3 LM3S5R31 và m ột master làm tr ạm điề u hành trung tâm s ử d ụng vi điề u khiể n ARM Cortex M4 LM4F232 c ủa hãng Texas Instrument. Master và các robot slave s ẽ giao ti ế p vớ i nhau thông qua sóng RF s ử d ụng module eZ430-RF2500 theo giao th ức được quy đị nh. Luận văn đã thự c hiện đượ c mô hình h ệ thố ng khá ổn định. Trong đó các robot slave đã vận chuyển các gói hàng đúng theo mong mu ố n của ngườ i vận hành và có kh ả năng tránh va chạm ở các giao l ộ. Chương trình trên máy tính thể hiện chính xác v ị trí robot ở lần cậ p nh ật vị trí cuố i cùng trên sàn làm vi ệc. Giao th ứ c mạng đượ c thiế t lậ p cho phép master có th ể kiể m soát sự t ồn t ại của các slave trong m ạng, t ừ đó sẽ có các quy ết định phân ph ối làn đườ ng hợ p lý. Giao thứ c mạng t ạo ra các đị a chỉ ứ ng vớ i t ừng đối tượ ng trong m ạng cho phép master và slave giao ti ế p vớ i nhau nhanh chóng và chính xác. Tuy còn m ột s ố h ạn ch ế nhưng đề tài đã tạo đượ c m ột mô hình h ệ th ố ng v ận chuyể n hàng hóa t ự động khá ổn định và có nhi ề u khả năng ứ ng d ụng nếu đượ c tiế p t ục phát triể n.

iii

MỤC LỤC Trang bìa Nhiệm vụ luận văn tốt nghiệ p Lờ i cảm ơn

i ii

Tóm tắt Luận văn Mục lục Danh sách hình ảnh

iv vii

Danh sách b ảng biểu

x

iii

CHƯƠNG 1 TỔNG QUAN .............................................................................................. 1 1.1 Sơ lượ c về robot: ...................................................................................................... 1 1.2 Robot tự hành: .......................................................................................................... 2 1.2.1 Robot tự hành theo đườ ng lậ p trình sẵn: ........................................................3 1.2.2 Tình hình nghiên c ứu và ứng dụng trên thế giớ i:........................................... 3 1.2.3 Tình hình nghiên c ứu và ứng dụng ở Việt Nam: ...........................................4

CHƯƠNG 2 PHÂN TÍCH NHIỆM VỤ LUẬN VĂN..................................................... 5 2.1 Các vấn đề cần giải quyết ......................................................................................... 5

2.2 Hướ ng giải quyết đề nghị ......................................................................................... 5 2.3 Phạm vi của luận văn ................................................................................................ 6

2.4 Sơ đồ tổng quát mô tả các module s ử dụng trong luận văn. ..................................... 6 CHƯƠNG 3 CƠ SỞ LÝ THUYẾT .................................................................................. 8 3.1 Giớ i thiệu về Module TI eZ430-RF2500: ................................................................8 3.1.1 Giớ i thiệu về Module RF Transceiver CC2500: ............................................ 8 3.1.2 Một vài thông s ố nổi bật của Module RF Transceiver CC2500: ................... 9 3.1.3 Giớ i thiệu về Module TI eZ430-RF2500: ......................................................9 3.1.4 Giớ i thiệu về MCU TI MSP430F2274: ....................................................... 10 3.2 Giớ i thiệu PIF 5R31 Development Kit: .................................................................. 11 3.2.1 Giớ i thiệu MCU TI Stellaris LM3S5R31: ................................................... 12 3.2.2 Module System Timer:................................................................................. 13 3.2.3 Module General Timer: ................................................................................ 14 3.2.4 Module Pulse Width Modulator (PWM): .................................................... 15 iv

3.2.5 Module Quadrature Encoder Interface (QEI): ............................................. 16 3.2.6 Module Universal Asynchronous Receivers/Transmitters (UARTs): ......... 17 3.2.7 Module Analog-to-Digital Converter (ADC): ............................................. 18 3.3 Giớ i thiệu TI EKS-LM4F232 Evaluation Kit:........................................................ 19 3.3.1 Giớ i thiệu MCU TI Stellaris LM4F232: ...................................................... 21 3.3.2 Module General Timer (GPTM): ................................................................. 23 3.3.3 Module Universal Asynchronous Receivers/Transmitters (UARTs): ......... 24 3.4 Giớ i thiệu Module RFID: ....................................................................................... 25 3.4.1 Khái ni ệm RFID:..........................................................................................25

3.4.2 Các phương thứ c truyền thông của công ngh ệ RFID: ................................. 25 3.4.3 Các thành ph ần của một hệ thống RFID: ..................................................... 26 3.4.4 Giớ i thiệu các thành ph ần RFID sử dụng trong luận văn: ........................... 26

3.5 Động cơ DC Servo: ................................................................................................ 27 3.5.1 Giớ i thiệu: .................................................................................................... 27

3.5.2 Điều khiển động cơ DC servo: ..................................................................... 28 3.5.3 Thuật toán PID: ............................................................................................ 28 3.5.4 Mạch cầu H: ................................................................................................. 32

3.6 Điều khiển robot bám theo line: ............................................................................. 35 3.6.1 Nguyên t ắc phân biệt line đen và trắng: ....................................................... 35 3.6.2 Nguyên t ắc điều khiển robot bám theo line: ................................................ 37 3.6.3 Áp dụng thuật toán PID vào dò line:............................................................ 39

CHƯƠNG 4 THIẾT KẾ VÀ THI CÔNG ..................................................................... 40 4.1 Mạch điện tử: .......................................................................................................... 40 4.1.1 Mạch dò line: ............................................................................................... 40 4.1.2 Mạch cầu H: ................................................................................................. 42 4.1.3 Mạch Stamp slave: ....................................................................................... 44 4.2 Thiết k ế và thi công cơ khí: ....................................................................................46 4.2.1 Thiết k ế và thi công sàn làm vi ệc của robot:................................................ 46 4.2.2 Thi công mô hình robot:............................................................................... 50 4.3 Xây dựng giải thuật giao tiế p trong mạng không dây: ........................................... 51 4.3.1 Các quy t ắc giao tiế p trong mạng: ............................................................... 51 v

4.3.2 Cấu trúc frame truyền trong mạng: ..............................................................51 4.4 Giải thuật chương trình vi điều khiển:.................................................................... 53 4.4.1 Giải thuật chương trình MCU trên module eZ430-RF2500: ....................... 53 4.4.2 Giải thuật chương trình trên vi điều khiển của Master: ............................... 57 4.4.3 Giải thuật chương trình trên vi điều khiển của Slave:.................................. 62

4.5 Chương trình trên máy tính: ...................................................................................76 4.5.1 Chương trình viế t bằng ngôn ngữ C#: ......................................................... 77 4.5.2 Chương trình viết bằng ngôn ngữ Python 2.7.3:.......................................... 78 CHƯƠNG 5 VẬN HÀNH VÀ KẾT QUẢ ..................................................................... 79 5.1 Vận hành:................................................................................................................ 79 5.2 K ết quả: .................................................................................................................. 82

CHƯƠNG 6 KẾT LUẬN ................................................................................................ 83 6.1 Những k ết quả đạt đượ c: ........................................................................................ 83 6.2 Khả năng ứng dụng: ............................................................................................... 83 6.3 Những hạn chế còn t ồn tại và hướ ng giải quyết: .................................................... 83 6.4 Hướ ng phát triển của đề tài: ................................................................................... 85

TÀI LIỆU THAM KHẢO: ................................................................................................ 86 PHỤ LỤC

..................................................................................................................... 87

vi

DANH SÁCH HÌNH ẢNH Hình 1.1 Hình ảnh một số robot.............................................................................................1 Hình 1.2 Hình ảnh một số robot tự hành................................................................................2 Hình 1.3 Một số hình ảnh về hệ thống robot Kiva.................................................................4

Hình 2.1 Sơ đồ tổng quát hệ thống ........................................................................................6 Hình 3.1 Module CC2500 ...................................................................................................... 8

Hình 3.2 Sơ đồ khối bên trong module CC2500.................................................................... 9 Hình 3.3 Module eZ430-RF2500......................................................................................... 10

Hình 3.4 Sơ đồ khối bên trong MSP430F2274.................................................................... 11 Hình 3.5 Kit phát tri ển PIF 5R31 ......................................................................................... 11

Hình 3.6 Sơ đồ khối vi điều khiển LM3S5R31 ................................................................... 13 Hình 3.7 Sơ đồ khối Module General Timer ....................................................................... 14 Hình 3.8 Sơ đồ khối module PWM ...................................................................................... 16 Hình 3.9 Sơ đồ khối bộ phát xung ....................................................................................... 16 Hình 3.10 Sơ đồ khối bộ QEI...............................................................................................17 Hình 3.11 Sơ đồ khối bộ UART .......................................................................................... 18 Hình 3.12 Sơ đồ khối bộ ADC............................................................................................. 19 Hình 3.13 EKS-LM4F232 Evaluation Kit ........................................................................... 20

Hình 3.14 Sơ đồ khối EKS-LM4F232 Evaluation Kit......................................................... 20 Hình 3.15 Sơ đồ khối vi điều khiển LM4F232 .................................................................... 22

Hình 3.16 Sơ đồ khối module General Timer LM4F232..................................................... 23 Hình 3.17 Sơ đồ khối bộ UART LM4F232 ......................................................................... 24 Hình 3.18 Module Reader RFID ACE630........................................................................... 26 Hình 3.19 Frame dữ liệu của module Reader RFID ACE630 ............................................. 26 Hình 3.20 Thẻ (Tag) 125 kHz .............................................................................................. 27

vii

Hình 3.21 Mô hình m ột hệ thống servo ............................................................................... 27

Hình 3.22 Động cơ DC servo sử dụng trong luận văn ......................................................... 28 Hình 3.23 Bộ điều khiển PID............................................................................................... 32 Hình 3.24 Thi ết k ế cơ bản của mạch cầu H ......................................................................... 32 Hình 3.25 Mạch cầu H chế độ thuận.................................................................................... 33 Hình 3.26 Mạch cầu H chế độ nghịch.................................................................................. 33

Hình 3.27 Sơ đồ cấu tạo của IR2184 ................................................................................... 34 Hình 3.28 Quang tr ở ............................................................................................................35 Hình 3.29 Mạch ứng dụng quang tr ở ................................................................................... 36 Hình 3.30 Cách s ắ p xế p quang tr ở và Led trên mạch dò line .............................................. 37 Hình 3.31 Mạch dò line s ử dụng 4 cảm biến .......................................................................37

Hình 3.32 Trườ ng hợ p robot chạy giữa line ........................................................................ 38 Hình 3.33 Trườ ng hợ p robot chạy lệch phải ....................................................................... 38 Hình 3.34 Trườ ng hợ p robot chạy lệch trái ......................................................................... 39 Hình 4.1 Sơ đồ nguyên lý mạch dò line ............................................................................... 41 Hình 4.2 Hình th ực tế mạch dò line ..................................................................................... 41

Hình 4.3 Sơ đồ nguyên lý mạch cầu H ................................................................................ 43 Hình 4.4 Hình th ực tế mạch cầu H....................................................................................... 43

Hình 4.5 Sơ đồ khối mạch Stamp slave ............................................................................... 45 Hình 4.6 Hình th ực tế mạch stamp và k ết nối vớ i board MCU slave .................................. 46 Hình 4.7 Bản thiết k ế một block .......................................................................................... 47 Hình 4.8 Số thứ tự các point trong 1 block .......................................................................... 48

Hình 4.9 Kích thướ c chi tiết sàn làm việc của robot ............................................................ 49 Hình 4.10 Vị trí các point trên sàn làm vi ệc của robot ........................................................ 49

Hình 4.11 Mô hình cơ khí robot slave .................................................................................50

viii

Hình 4.12 Mô hình robot slave hoàn ch ỉnh.......................................................................... 50 Hình 4.13 Cấu trúc frame hoàn ch ỉnh .................................................................................. 51 Hình 4.14 Frame slave truyền để đăng ký sự hiện diện .......................................................52 Hình 4.15 Frame slave truyền vị trí hiện tại của nó ............................................................. 52 Hình 4.16 Frame slave truyền mã số gói hàng..................................................................... 52 Hình 4.18 Frame slave tr ả lờ i xác nhận việc đã nhận frame thành công ............................. 52 Hình 4.19 Frame master truy ền kiểm tra sự tồn tại của slave .............................................. 52 Hình 4.20 Frame master truy ền yêu c ầu slave dừng/chạy ...................................................53 Hình 4.21 Frame master truy ền địa chỉ kho của gói hàng ................................................... 53 Hình 4.22 Frame master truy ền xác nh ận việc nhận 1 frame thành công............................ 53 Hình 4.23 Frame truyền từ module eZ430-RF2500 đến master/slave................................. 54 Hình 4.24 Frame truyền từ master/slave đến module eZ430-RF2500 ................................. 54 Hình 4.25 Mô hình giao ti ế p giữa master vớ i các module ................................................... 57 Hình 4.26 Các module s ử dụng trên vi điều khiển của master ............................................ 57 Hình 4.27 Các module s ử dụng trên vi điều khiển của robot slave ..................................... 63

Hình 4.28 Quy đị nh về biến định vị hướ ng của robot ......................................................... 71 Hình 4.29 Giao diện chương trình viết trên C# ................................................................... 77 Hình 4.30 Cửa sổ thiết lậ p k ết nối UART............................................................................77 Hình 4.31 Cửa sổ thay đổi vị trí kho hàng ........................................................................... 78 Hình 4.32 Cửa sổ chương trình trên Python ........................................................................78

ix

DANH SÁCH CÁC BẢNG BIỂU Bảng 3.1 Bảng đặc điểm các tần số của sóng radio ........................................................ 25

x

TỔ NG QUAN

CHƯƠNG 1

TỔNG QUAN

1.1 Sơ lượ c về robot: Robot được định nghĩa là một máy cơ khí đượ c thiết k ế để làm các công vi ệc thay cho con ngườ i. Các bộ ph ận trên robot ứng d ụng cơ điện t ử và được điều khiển b ở i một chương trình máy tính hoặc các vi mạch điện t ử đượ c lậ p trình. Robot có th ể đượ c phân loại ra các dạng tự

động hoàn toàn, bán t ự động và điều khiển từ xa. Trên thế giới ngày nay, robot đượ c ứng dụng trong r ất nhiều lĩnh vực, và ph ụ thuộc vào từng lĩnh vực s ẽ yêu cầu robot có các thi ết k ế, c ấu trúc và ch ức năng khác nhau. Từ đó robot có thể đượ c phân loại thành các d ạng chính như: robot tự hành (robot di độ ng, mobile robot), robot công nghiệ p, robot cộng tác (collaborative robot), robot quân s ự.

Hình 1.1 Hình ảnh một số robot

1

TỔ NG QUAN

1.2 Robot tự hành: Trong các loại robot trên thì robot tự hành đượ c ứng dụng r ộng rãi nhất trong mọi lĩnh vực, mọi môi trườ ng. Theo lý thuy ết, môi trườ ng hoạt động của robot tự hành có thể là đất, nướ c, k hông khí, không gian vũ tr ụ hay sự 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à 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, gần như luôn đả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), 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), và loại hỗn hợ p bánh xe và xích (ít gặ p).

Hình 1.2 Hình ảnh một số robot t ự hành

2

TỔ NG QUAN 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ố, khoan 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... 1.2.1 Robot t ự hành theo đườ ng lậ p trình sẵ n:

Một ứng dụng đượ c nghiên cứu từ r ất sớ m của robot tự hành đó là robot tự hành theo

đườ ng lậ p trình sẵn (automatic guided vehicle (AGV)). AGV là robot t ự hành di chuyển theo những đườ ng line hoặc những điểm mốc trên sàn nhà ho ặc sử dụng laser ho ặc xử lý ảnh. Loại

robot này đượ c ứng dụng nhiều nhất trong công nghi ệp để vận chuyển các vật liệu xung quanh những máy sản xuất hoặc trong nhà kho. Trong ứng dụng robot vận chuyển vật liệu

này, điều khó khăn nhất đó là việ c d ẫn đườ ng cho robot . Thông thườ ng n ếu nhà máy ch ỉ có một robot tự hành thì vi ệc robot di chuyển theo tuyến đườ ng cố định khá đơn giản. Tuy nhiên khi nhà máy có nhi ều robot tự hành di chuyển trùng tuyến đườ ng vớ i nhau thì việc dẫn đườ ng cho từng robot di chuyển lại khó khăn hơn rấ t nhiều. 1.2.2 Tình hình nghiên c ứ u và ứ ng d ụng trên thế giớ i:

Việc phát triển h ệ th ống robot tự hành t ự định v ị và dẫn đường đã đượ c r ất nhiều công ty,

trường đại học trên thế gi ớ i nghiên cứu nhưng đế n nay ch ỉ có h ệ thống robot Kiva do công ty Kiva Systems phát tri ển và sản xuất là gây đượ c ti ếng vang trên thế giớ i. Hiện h ệ thống này

đã đượ c s ử d ụng trong kho hàng c ủa m ột s ố công ty lớ n trên thế gi ới như hệ th ống c ửa hàng Amazon.

3

TỔ NG QUAN

Hình 1.3 M ột số hình ảnh về hệ thố ng robot Kiva 1.2.3 Tình hình nghiên c ứ u và ứ ng d ụng ở Việt Nam:

Ở Vi ệt Nam hiện nay hệ th ống robot tự động vận chuyển hàng hóa trong s ản xu ất cũng đã đượ c áp dụng trong một số nhà máy l ớn như nhà máy gạch Đồng Tâm ở dạng đơn giản. Hệ th ống robot tự động v ận chuyển hàng hóa có kh ả năng ứng dụng vào bất c ứ ngành nào có nhu cầu vận chuyển số lượ ng lớn hàng hóa đế n nhiều điểm khác nhau như trong ngành hàng không chuyển các valy hành lý c ủa hành khách v ề máy bay, trong các nhà máy c ần vận chuyển nguyên li ệu đến các máy khác nhau ho ặc vận chuyển thành phẩm đến chứa ở nhiều kho riêng biệt. Nói tóm lại ngày nay nhu c ầu ứng d ụng h ệ th ống v ận chuyển hàng hóa t ự động là r ất l ớ n. Vì vậy vi ệc nghiên cứu và ứng d ụng thành công h ệ thống v ận chuyển này ở Việt Nam có ý

nghĩa rất l ớ n trong việc áp dụng khoa học k ỹ thu ật giải phóng sức lao động của con ngườ i và nâng cao năng suất lao độ ng.

4

PHÂN TÍCH NHIỆM VỤ LUẬN VĂN

CHƯƠNG 2

PHÂN TÍCH NHI ỆM VỤ LUẬN VĂN

Luận văn nhằm xây dựng một hệ thống các robot t ự động vận chuyển hàng hóa t ừ một tổng

kho đến các kho ph ụ tùy thuộc vào gói hàng và có kh ả năng chọn đượ c tuyến đườ ng ít xung đột nhất. Hệ thống cơ bản gồm một trung tâm điều khiển (master) và các slave.

2.1 Các vấn đề cần giải quyết 

Làm sao để các robot slave di chuy ển giữa các kho hàng.



Làm sao để các robot slave không va ch ạm nhau tại các giao l ộ.



Làm sao để các robot slave nh ận ra gói hàng.



Làm sao để ngườ i s ử d ụng có thể điều khiển phân phối v ị trí kho hàng cho các gói

hàng dễ dàng.

2.2 Hướ ng giải quyết đề nghị 

Để di chuyển giữa các kho hàng thì tôi l ựa chọn phương pháp dò line. Phương pháp

này đượ c lựa chọn vì tính dễ sử dụng, tính chính xác và có l ợ i về mặt kinh tế. 

Để các robot slave có kh ả năng phát hiện ra nhau để tránh va ch ạm thì ta sử dụng

một master quản lý vị trí các robot slave. Khi master phát hi ện ra có hai robot đang đế n cùng một giao lộ thì master sẽ ra l ệnh cho robot đế n giao lộ sau (thông báo lên master mu ộn hơn) phải dừng lại chờ robot đến trướ c qua khỏi giao lộ thì robot đến sau s ẽ đượ c tiế p tục chạy. Một vấn đề phát sinh là làm sao các robot slave có th ể truyền vị trí của nó lên master x ử lý

xung đột? Đây là mộ t bài toán về định vị và d ẫn đườ ng (localization, navigation). Trong lu ận văn này tôi lựa chọn các lưu bản đồ sàn làm việc trên robot slave và vi ệc dẫn đường cũng đượ c thực hiện trên slave, master ch ỉ chỉ ra một số mốc trong việc dẫn đườ ng, còn lại sẽ đượ c xử lý hoàn toàn trên slave. Từ đây lại n ảy sinh ra một v ấn đề n ữa là làm sao để các slave có th ể giao tiế p v ớ i master? Một đòi hỏi bắt buộc là giao tiế p phải là không dây và t ốc độ phải đủ nhanh cho phép master cậ p nhật các yêu c ầu từ slave và ra l ệnh cho slave nhanh chóng, đáp ứ ng yêu cầu thờ i gian thực. Ở lu ận văn này tôi chọn phương án sử d ụng mạng RF dùng module eZ430-RF2500 c ủa

TI để thực hiện. 5

PHÂN TÍCH NHIỆM VỤ LUẬN VĂN 

Để nhận di ện các gói hàng thì ở luận văn này tôi sử dụng công ngh ệ RFID vớ i mỗi

gói hàng tương ứ ng vớ i một thẻ Tag RFID. 

Để ngườ i sử dụng giao tiế p vớ i master một cách tr ực quan thì s ẽ có chương trình

trên máy tính cho phép ngườ i sử dụng có thể cấu hình vị trí kho hàng cho các gói hàng d ễ dàng.

2.3 Phạm vi của luận văn 

Xây dựng mô hình robot slave có kh ả năng di chuyể n theo line một cách hoàn

chỉnh. 

Xây dựng mô hình tr ạm điều khiển master.



Xây dựng cấu trúc mạng không dây dùng giao ti ế p giữa các slave v ớ i master mà s ẽ

ứng dụng cấu trúc đó vào mạ ng RF trong luận văn này. 

Lậ p trình phần mềm cho phép tương tác giữa ngườ i dùng và master m ột các tr ực

quan nhất.

2.4 Sơ đồ tổng quát mô t ả các module sử dụng trong lu ận văn

Hình 2.1 Sơ đồ t ổ ng quát h ệ thố ng

Board Slave s ẽ điều khiển hai động cơ của robot thông qua m ạch công su ất c ầu H sao cho robot bám theo line đượ c thiết lậ p sẵn. Board slave s ẽ đọc tín hiệu từ cảm biến dò line sử dụng ADC. Board Slave giao ti ế p v ớ i module reader ACE630 và module eZ420-RF2500 qua chuẩn UART.

6

PHÂN TÍCH NHIỆM VỤ LUẬN VĂN Board master s ẽ nhận bản tin từ slave nhờ giao tiế p vớ i board eZ430-RF2500 s ử dụng chuẩn UART và giao ti ế p với máy tính cũng sử dụng chuẩn UART. Master và slave s ẽ giao tiế p vớ i nhau thông qua môi trườ ng RF sử dụng module eZ430RF2500 của hãng Texas Instrument.

7

CƠ SỞ LÝ THUYẾT

CHƯƠNG 3


Chương 2 đã trình bày các vấn đề sẽ đượ c nghiên cứu trong luận văn cũng như đề xuất các giải pháp thực hiện. Chương này sẽ giớ i thiệu các lý thuyết liên quan, trình bày các phương pháp nghiên c ứu và thực hành cụ th ể, đồng thời phân tích, đánh giá, làm rõ và lý giả i cho sự các lựa chọn đó.

3.1 Giớ i thiệu về Module TI eZ430-RF2500: Vớ i yêu cầu phải l ựa chọn một h ệ thống truyền nh ận không dây để ứng d ụng vào luận văn này, em lựa chọn mạng RF sử dụng module eZ430-RF2500 c ủa hãng Texas Instrument do tính dễ sử dụng và ổn định của sản phẩm này. Khi cần mở r ộng hệ thống có thể chọn mạng WiFi giao thức TCP/IP để có khoảng cách và tốc độ truyền nhận cao hơn. Trong khuôn khổ luận văn này, do hệ thống nhỏ và thờ i gian phát triển ngắn nên chỉ dừng lại ở hệ thống sử dụng mạng RF. 3.1.1 Giớ i thiệu về Module RF Transceiver CC2500:

CC2500 là một bộ thu phát 2.4 GHz đượ c thiết k ế cho những ứng dụng không dây ti ết kiệm năng lượ ng. Bộ thu phát này đượ c tích hợ p b ộ điều bi ến d ải nền có thể tùy chỉnh đượ c

cho phép thay đổ i tốc độ truyền lên đến 500 kBaud. Những thông số hoạt động và bộ nhớ truyền nhận (FIFO) của CC2500 có th ể được điều khiển thông qua giao ti ế p SPI (Serial Peripheral Interface). Thông thường, CC2500 đượ c sử dụng cùng với 1 vi điều khiển và một vài linh kiện thụ động trong hệ thống.

Hình 3.1 Module CC2500

8


Hình 3.2 Sơ đồ khố i bên trong module CC2500 3.1.2 M ột vài thông s ố nổ i bật của Module RF Transceiver CC2500: −

Độ nhạy cao (-104 dBm ở tốc độ 2.4 kBaud, t ỷ lệ lỗi gói 1%).

−

Tiêu thụ ít năng lượ ng (dòng tiêu th ụ 13.3 mA ở tốc độ 250 kBaud).

−

Khả năng chọn lọc dữ liệu thu đượ c.

−

Có thể thay đổi tốc độ từ 1.2 đến 500 kBaud.

−

Dải tần hoạt động: 2400 – 2483.5 MHz.

3.1.3 Giớ i thiệu về Module TI eZ430-RF2500:

eZ430-RF2500 là m ột module giao ti ế p USB và truyền nhận dữ liệu không dây do hãng Texas Instrument s ản xuất. eZ430-RF2500 đượ c phát triển dựa trên MSP430F2274 k ết hợ p vớ i chip CC2500 truyền nhận tín hiệu ở vùng tần số 2.4 GHz, tích h ợ p sẵn cảm bi ến nhiệt độ. Do đặc điểm dòng MSP430 có nh ững ưu thế về năng lượ ng nên đáp ứng đượ c những nhu c ầu trong thiết bị di động. Bộ eZ430-RF2500T là một sản phẩm có thể làm việc vớ i đầu cắm USB như một hệ thống độc lậ p vớ i cảm biến bên ngoài. Ho ặc sử dụng thiết k ế mở r ộng vớ i những module ngoại vi. Vớ i giao diện gỡ lỗi USB cho phép sản phẩm có thể truyền và nhận dữ li ệu t ừ xa từ máy tính cá nhân s ử d ụng truyền nhận nối tiế p UART.

9


Hình 3.3 Module eZ430-RF2500 3.1.4 Giớ i thiệu về MCU TI MSP430F2274:

MSP430F2274 là vi điều khiển 16bit tiết kiệm năng lượ ng thuộc dòng MSP430 của hãng Texas Instrument vớ i một số thông số nổi bật sau: −

Nguồn điện thế sử dụng trong kho ảng từ 1.8V đến 3.6V.

−

Nguồn tiêu thụ cực thấ p. o

Chế độ hoạt động: 270 µA tại 1MHz và 2.2V.

o

Chế độ ngủ: 0.7 µA.

o

Chế độ không hoạt động: 0.1 µA.

−

Thờ i gian thức dậy từ chế độ ngủ là ít hơn 1µs.

−

Cấu trúc RISC 16 bit, mỗi chu k ỳ lệnh hoạt động mất 62.5ns.

−

Có hai thanh ghi cho Timer_A, Timer_B 16 bit.

−

Hỗ tr ợ các giao di ện giao tiế p nối tiếp như: UART, SPI, I2C, IrDA.

−

Bộ chuyển đổi ADC 10-bit vớ i tốc độ chuyển đổi lên tớ i 200ksps.

−

Bộ nhớ bao gồm 32KB + 256B Flash Memory, 1KB RAM.

10


Hình 3.4 Sơ đồ khố i bên trong MSP430F2274

3.2 Giớ i thiệu PIF 5R31 Development Kit: Kit phát triển PIF 5R31 Development Kit s ử dụng vi điều khiển TI Stellaris LM3S5R31

đượ c sử dụng làm bộ điều khiển trung tâm của robot trong luận văn này.

Hình 3.5 Kit phát triể n PIF 5R31

11

CƠ SỞ LÝ THUYẾT 3.2.1 Giớ i thiệu MCU TI Stellaris LM3S5R31:

MCU LM3S5R31 là m ột vi điều khiển 32 bit của hãng Texas Instrument s ử dụng lõi ARM Cortex-M3 có kh ả năng hoạt động ở t ốc độ nhân là 80 MHz cùng nhi ều các module ngo ại vi mở r ộng thích hợ p cho các ứng dụng điều khiển chuyển động. 

Lõi xử lý ARM Cortex-M3.



Tốc độ cao: hoạt động ở 80-MHz.



Bộ nhớ Flash 256 KB.



Bộ nhớ RAM 48 KB.



Bộ nhớ ROM nội nạ p sẵn thư viện StellarisWare.



Giao tiế p ngoại vi mở r ộng (External Peripheral Interface).



Các chuẩn giao tiế p cao cấ p: UART, SSI, I2C, I2S, CAN, USB.



Tích hợ p sẵn: bộ định thời đa chức năng (general-purpose timers), bộ định thờ i

watchdog (watchdog timers), DMA, gerenal-purpose I/Os. 

Điều khiển chuyển động s ử d ụng các module tích h ợ p s ẵn: Pulse Width Modulator

(PWM) và bộ đọc encoder (quadrature encoder inputs). 

Chuẩn JTAG và ARM Serial Wire Debug (SWD).



Chuẩn đóng gói 100 -pin LQFP.

12


Hình 3.6 Sơ đồ khố i vi điề u khiể n LM3S5R31 3.2.2 Module System Timer:

Bộ định thờ i h ệ th ống đượ c tích hợ p s ẵn, là bộ định thời 24 bit, đếm xu ống. Bộ định thờ i

đượ c sử dụng trong nhiều ứng dụng như: −

Bộ định thờ i cho RTOS. 13

CƠ SỞ LÝ THUYẾT −

Bộ định thờ i thờ i gian.

−

Bộ đếm đơn giản sử dụng để đo thờ i gian hoàn t ất hay thời gian đã sử dụng.

3.2.3 Module General Timer:

4 b ộ General-Purpose Timer Modules (GPTM), m ỗi bộ cung cấ p hai bộ timer/counter 16 bit. Mỗi GPTM có thể đượ c cấu hình để hoạt động độ c lậ p: −

Là một bộ đếm thờ i gian 32-bit.

−

Là một đồng hồ thờ i gian thực 32- bit (RTC) để nắm bắt sự kiện.

−

Dùng cho điề u chế r ộng xung (PWM).

−

Để kích hoạt chuyển đổi tương tự - số.

Hình 3.7 Sơ đồ khố i Module General Timer 

Chế độ Timer 32-bit: o

Có thể lậ p trình one-shot thờ i gian.

o

Có thể lậ p trình theo chu k ỳ thờ i gian.

o

Real-Time Clock s ử dụng một xung nhị p 32,768-KHz bên ngoài như đầ u vào.

o o

Ngườ i dùng có thể dừng trong quá trình g ỡ r ối. Kích hoạt ADC.

14

CƠ SỞ LÝ THUYẾT 

Chế độ Timer 16-bit: o

Chức năng như độ định thờ i v ớ i m ột b ộ chia 8-bit (cho chế độ one-shot và chế độ theo chu k ỳ).

o

Có thể lậ p trình one-shot thờ i gian.

o

Có thể lậ p trình theo chu k ỳ thờ i gian.

o o





Ngườ i dùng có thể dừng trong quá trình g ỡ r ối Kích hoạt ADC.

Chế độ Input Capture 16-bit: o

Capture đầu vào theo sườ n.

o

Capture đầu vào theo thơi gian.

Chế độ PWM 16bit: o

Chế độ PWM đơn giản vớ i đầu ra có thể lậ p trình phần mềm của tín hiệu PWM.

3.2.4 Module Pulse Width Modulator (PWM):

LM3S5R31 có 1 bộ PWM gồm 4 bộ phát xung và 1 b ộ điều khiển cho tổng cộng 8 ngõ ra PWM. Bộ điều khiển sẽ quyết định m ức tích cực của tín hiệu PWM và tín hiệu nào ra đượ c xuất ra ở chân nào. Mỗi bộ phát xung s ẽ cho 2 tín hi ệu PWM sử dụng chung 1 b ộ định thờ i và cùng t ần số. Một số đặc điểm chú ý c ủa bộ phát xung: −

Bộ định thờ i 16 bit: o

2 chế độ hoạt động: đếm xuống hoặc đếm lên/xuống.

o

Cậ p nhật giá tr ị ban đầu cho bộ định thờ i có thể đồng bộ vớ i các bộ phát xung khác.

−

Tín hiệu PWM ngõ ra có th ể đượ c c ấu hình dựa trên tín hi ệu t ừ b ộ định thờ i hoặc

tín hiệu từ bộ so sánh. −

Tạo đượ c Dead-band cho 2 tín hi ệu PWM ngõ ra.

15


Hình 3.8 Sơ đồ khố i module PWM

Hình 3.9 Sơ đồ khố i bộ phát xung 3.2.5 Module Quadrature Encoder Interface (QEI):

Bộ QEI vớ i khả năng đọc tín hiệu từ encoder tăng (incremental encoder) 2 kênh và cho biết chiều quay, v ận tốc góc của động cơ. Đầ u vào c ủa bộ QEI là 2 kênh xung encoder và kênh xung index (có th ể có hoặc không).

16

CƠ SỞ LÝ THUYẾT Stellaris LM3S5R31 có sẵn 2 b ộ QEI cho phép điề u khiển 2 động cơ cùng lúc vớ i những

đặc điểm sau: −

Bộ tích hợ p vị trí cho phép theo dõi v ị trí của encoder.

−

Bộ lọc nhiểu đầu vào có thể lập trình đượ c.

−

Bộ định thờ i tích hợ p sẵn cho phép đo vậ n tốc.

−

Tần số xung ngõ vào có th ể lên đến ¼ tần số hoạt động của lõi.

−

Có thể lậ p trình ngắt khi: o

Có xung chân Index.

o

Tràn bộ định thờ i vận tốc.

o

Thay đổi hướ ng.

o

Xảy ra lỗi.

Hình 3.10 Sơ đồ khố i bộ QEI 3.2.6 Module Universal Asynchronous Receivers/Transmitters (UARTs):

Stellaris LM3S5R31 có 3 b ộ UART với các đặc điểm: −

Bộ tạo tốc độ baud có th ể lập trình đượ c.

−

Bộ FIFO 16x8 bit đọ c và ghi phân bi ệt nhằm giúp vi điều khiển giảm số lần vào

−

Các mức độ FIFO 1 / 8, 1 / 4, 1 / 2, 3 / 4, và 7 / 8.

ngắt.

17


Tiêu chuẩn giao tiếp không đồ ng bộ vớ i các bit bắt đầu, dừng lại, và tính chẵn lẻ.

−

Phát hiện và sửa lỗi đườ ng truyền.

−

Lập trình đầy đủ các đặc tính giao di ện nối tiế p: o

5, 6, 7, hoặc 8 bit dữ liệu

o

Tạo và kiểm tra bit chẵn, lẻ hoặc không

o

Tạo 1 hoặc 2 bit dừng

Hình 3.11 Sơ đồ khố i bộ UART 3.2.7 Module Analog-to-Digital Converter (ADC):

Stellaris LM3S5R31 có 2 b ọ ADC độ phân giải 10 bit và hỗ tr ợ đến 16 kênh đầ u vào và 1 cảm biến nhiệt nội. Mỗi b ộ ADC gồm 4 b ộ sequencer cho phép l ấy mẫu t ừ nhiều đầu vào mà không cần tín hiệu điều khiển liên tục. Mỗi bộ sequencer cung c ấ p khả năng cài đặt đầy đủ từ nguồn l ấy mẫu, s ự ki ện b ắt đầu l ấy mẫu, ngắt đến mức độ ưu tiên của các sequencer. M ốc sự kiện bắt đầu lấy mẫu có thể lấy t ừ phần mềm (lậ p trình), bộ định thờ i, bộ so sánh tương tự (analog), PWM, GPIO. Một vài thông s ố quan tr ọng của bộ ADC: 18


Lên đến 16 kênh tín hi ệu đầu vào.

−

Có thể cấu hình đầu vào là tín hiệu dạng vi phân.

−

Có cảm biến nhiệt nội.

−

Tốc độ lấy mẫu tối đa 1 triệu mẫu mỗi giây.

−

Có phần cứng lấy trung bình mẫu, cho phép l ấy trung bình đế n 64 mẫu.

Hình 3.12 Sơ đồ khố i bộ ADC

3.3 Giớ i thiệu TI EKS-LM4F232 Evaluation Kit: Kit phát triển TI EKS-LM4F232 Evaluation Kit s ử dụng vi điều khiển TI Stellaris

LM4F232 và đượ c sử dụng làm bộ điều khiển trung tâm kiểm soát hoạt động c ủa các robot trong luận văn này.

19


Hình 3.13 EKS-LM4F232 Evaluation Kit

Hình 3.14 Sơ đồ khố i EKS-LM4F232 Evaluation Kit

20

CƠ SỞ LÝ THUYẾT 3.3.1 Giớ i thiệu MCU TI Stellaris LM4F232:

MCU LM4F232 là m ột vi điều khiển 32 bit của hãng Texas Instrument s ử dụng lõi ARM Cortex-M4F có khả năng hoạt động ở tốc độ nhân là 80 MHz . Điểm khác bi ệt của lõi ARM Cortex-M4F vớ i lõi ARM Cortex-M3 là khả năng tính toán số chấm động. Lõi ARM Cortex-

M4F đượ c tích hợ p bộ Foating Point Unit (FPU) cho phép tính toán s ố chấm động nhanh hơn và chính xác hơn. 

Lõi xử lý ARM Cortex-M4F.



Tốc độ cao: hoạt động ở 80-MHz.



Bộ nhớ Flash 256 KB.



Bộ nhớ RAM 32 KB.



Bộ nhớ ROM nội nạ p sẵn thư viện StellarisWare.



Các chuẩn giao tiế p cao cấ p: UART, SSI, I2C, I2S, CAN, USB.



Tích hợ p sẵn: bộ định thời đa chức năng (general-purpose timers), bộ định thờ i

watchdog (watchdog timers), DMA, gerenal-purpose I/Os. 

Điều khiển chuyển động s ử d ụng các module tích h ợ p s ẵn: Pulse Width Modulator

(PWM) và bộ đọc encoder (quadrature encoder inputs). 

Chuẩn JTAG và ARM Serial Wire Debug (SWD).



Chuẩn đóng gói 144 -pin LQFP.

21


Hình 3.15 Sơ đồ khối vi điề u khiể n LM4F232

22

CƠ SỞ LÝ THUYẾT 3.3.2 Module General Timer (GPTM):

LM4F232 có 12 b ộ General-Purpose Timer Modules (GPTM), g ồm 6 bộ định thờ i 16/32 bit và 6 b ộ định thờ i 32/64 bit. Mỗi b ộ định thờ i 16/32 bit cung c ấ p hai bộ đếm 16 bit hoặc 1 bộ đếm 32 bit. Mỗi b ộ định thờ i 32/64 bit cung c ấ p hai bộ đếm 32 bit ho ặc 1 bộ đếm 64 bit. Mỗi GPTM có thể đượ c cấu hình để hoạt động độ c lậ p: −

Là một bộ đếm thờ i gian 32-bit.

−

Là một đồng hồ thờ i gian thực 32- bit (RTC) để nắm bắt sự kiện.

−

Dùng cho điề u chế r ộng xung (PWM).

−

Để kích hoạt chuyển đổi tương tự - số.

Hình 3.16 Sơ đồ khố i module General Timer LM4F232 

Chế độ hoạt động 16/32 bit: o

Có thể lậ p trình chạy 1 lần ở bộ đếm 16/32 bit.

o

Có thể lậ p trình theo chu k ỳ thờ i gian ở bộ đếm 16/32 bit.

o

Có thể lậ p trình bộ định thờ i 16 bit vớ i bộ chia tần 8 bit.

o

Real-Time Clock 32 bit s ử d ụng một xung nhị p 32,768-KHz bên ngoài như

đầu vào. 

Chế độ hoạt động 32/64 bit: o

Có thể lậ p trình chạy 1 lần ở bộ đếm 32/64 bit.

o

Có thể lậ p trình theo chu k ỳ thờ i gian ở bộ đếm 32/64 bit. 23

CƠ SỞ LÝ THUYẾT o

Có thể lậ p trình bộ định thờ i 32 bit vớ i bộ chia tần 16 bit.

o

Real-Time Clock 64 bit s ử d ụng một xung nhị p 32,768-KHz bên ngoài như

đầu vào. 3.3.3 Module Universal Asynchronous Receivers/Transmitters (UARTs):

Stellaris LM4F232 có 8 bộ UART với các đặc điểm: −

Bộ tạo tốc độ baud có th ể lập trình đượ c.

−

Bộ FIFO 16x8 bit đọ c và ghi phân bi ệt giúp vi điề u khiển giảm số lần vào ngắt.

−

Các mức độ FIFO 1 / 8, 1 / 4, 1 / 2, 3 / 4, và 7 / 8.

−

Tiêu chuẩn giao tiếp không đồ ng bộ vớ i các bit bắt đầu, dừng lại, và tính chẵn lẻ.

−

Phát hiện và sửa lỗi đườ ng truyền.

−

Lập trình đầy đủ các đặc tính giao di ện nối tiế p: o

5, 6, 7, hoặc 8 bit dữ liệu

o

Tạo và kiểm tra bit chẵn, lẻ hoặc không

o

Tạo 1 hoặc 2 bit dừng

Hình 3.17 Sơ đồ khố i bộ UART LM4F232

24


3.4 Giớ i thiệu Module RFID: 3.4.1 Khái niệm RFID:

RFID (Radio Frequency Identification) là công ngh ệ xác nh ận dữ liệu đối tượ ng bằng sóng vô tuyến để nhận dạng, theo dõi và lưu thông tin trong mộ t thẻ gọi là Tag. Reader quét d ữ liệu thẻ và gửi thông tin đến cơ sở dữ liệu lưu trữ dữ liệu của thẻ. K ỹ thuật RFID có liên quan đế n hệ thống không dây cho phép m ột thiết bị đọc thông tin

đượ c chứa trong một chip không tiế p xúc tr ực tiế p ở khoảng cách xa, mà không th ực hiện bất k ỳ giao tiế p vật lý nào hoặc yêu cầu một sự nhìn thấy giữa hai cái. Nó cho ta phương pháp truyền và nh ận dữ liệu từ một điểm đến điểm khác. Dạng đơn giản nh ất đượ c s ử d ụng hiện nay h ệ th ống RFID bị động làm việc như sau: một RFID reader truyền một tín hiệu tần số vô tuyến điện tử qua antenna c ủa nó đến một con chip không tiế p xúc. Reader nh ận thông tin tr ở lại từ chip và g ửi nó đến máy tính điề u khiển đầu

đọc và xử lý thông tin tìm đượ c từ con chip. Các con chip không ti ếp xúc, không tích điệ n, chúng hoạt động bằng cách sử dụng năng lượ ng chúng nh ận từ tín hiệu đượ c gửi bở i một reader. K ỹ thuật RFID sử dụng truyền thông không dây trong d ải tần s ố sóng vô tuyến để truyền dữ liệu từ các thẻ đến các reader. Th ẻ có thể được đính kèm hoặ c gắn vào đối tượng đượ c nhận dạng như sản phẩm, hộ p hoặc pallet. 3.4.2 Các phương thứ c truyề n thông của công ngh ệ RFID:

Công nghệ RFID sử d ụng 3 phương thứ c truyền thông dựa trên tần s ố c ủa sóng radio như bảng:

Bảng 3.1 Bảng đặc điểm các tần số của sóng radio Tần số

Đặc tính

Tần số thấ p

•

Sử dụng trong phạm vi ngắn và trung bình.

(100 – 500 kHz)

•

Chi phí thấ p.

•

Tốc độ đọc dữ liệu thấ p.

Tần số trung bình

•

Sử dụng trong phạm vi ngắn và trung bình.

(10 – 15 MHz)

•

Chi phí thấ p.

•

Tốc độ đọc dữ liệu trung bình. 25

CƠ SỞ LÝ THUYẾT Tần số cao

•

Sử dụng trong phạm vi bán kính r ộng.

(850 – 950 MHz; 2.4 – 5.8

•

Chi phí cao

•

Tốc độ đọc dữ liệu cao.

GHz)

3.4.3 Các thành ph ần của một hệ thố ng RFID:

Các thành ph ần chính trong h ệ thống RFID là thẻ (Tag), reader và cơ sở dữ liệu. 3.4.4 Giớ i thiệu các thành ph ần RFID sử d ụng trong lu ận văn: 

Module Reader RFID ACE630:

Module Reader RFID ACE630 có kh ả năng đọc đượ c các thẻ (Tag) ở tần số 125 kHz và giao tiế p vớ i hệ thống ngoài thông qua giao ti ế p UART.

Hình 3.18 Module Reader RFID ACE630



Frame dữ liệu của module Reader RFID ACE630:

15 Bytes Hình 3.19 Frame d ữ liệu của module Reader RFID ACE630 

Thẻ (Tag) 125 kHz:

Sử dụng cùng v ớ i bộ đọc RFID ACE630 là các th ẻ (Tag) 125 kHz.

26


Hình 3.20 Thẻ (Tag) 125 kHz

Trong luận văn có sử dụng 4 thẻ Tag vớ i mã số: 0008429816, 0015120796, 0008410126, 0006295720.

3.5 Động cơ DC Servo: 3.5.1 Giớ i thiệu:

Động cơ servo đượ c thiết k ế là một hệ thống h ồi tiế p vòng kín. Tín hi ệu ra của động cơ đượ c nối vớ i một mạch điều khiển. Khi động cơ quay, vậ n tốc và vị trí sẽ đượ c hồi tiế p về mạch điều khiển này. N ếu có bầt k ỳ lý do nào ngăn cả n chuyển động quay của động cơ, cơ cấu hồi tiế p sẽ nhận thấy tín hiệu ra chưa đạt đượ c vị trí mong muốn. Mạch điều khiển tiế p tục chỉnh sai l ệch cho động cơ đạt được điể m chính xác.

Động cơ servo có nhiề u kiểu dáng và kích thước, đượ c sử dụng trong nhiều máy khác nhau, từ máy tiện điều khiển b ằng máy tính cho đến các mô hình máy bay, xe hơi hay trong các robot.

Hình 3.21 Mô hình m ột hệ thố ng servo

27


Hình 3.22 Động cơ DC servo sử d ụng trong lu ận văn 3.5.2 Điề u khiển động cơ DC servo:

Để điều khiển tốc độ động cơ DC ta phải điề u khiển đượ c giá tr ị điện áp một chiều trên động cơ. Chúng ta s ẽ thực hiện việc biến đổi này thông qua m ột mạch cầu H (biến đổi điện áp DC – DC). Để ổn định vận tốc trên động cơ DC ta sử dụng thuật toán PID. 3.5.3 Thuật toán PID:

PID là cách vi ết tắc của các t ừ Propotional (tỉ lệ), Integral (tích phân) và Derivative (đạo hàm). Tuy xuất hi ện r ất lâu nhưng đến nay PID v ẫn là giải thuật điều khiển đượ c dùng nhi ều nhất trong các ứng dụng điều khiển tự động. Giải thuật tính toán bộ điều khiển PID bao gồm 3 thông số riêng biệt, do đó đôi khi nó còn

đượ c gọi là điều khiển ba khầu: các giá tr ị tỉ lệ, tích phần và đạo hàm, viết tắt là P, I và D. Giá tr ị tỉ l ệ xác định tác động c ủa sai số hi ện t ại, giá tr ị tích phân xác định tác động c ủa t ổng các sai số quá khứ, và giá tr ị vi phân xác định tác động của t ốc độ bi ến đổi sai số. T ổng chậ p c ủa

ba tác động này dùng để điều chỉnh quá trình thông qua m ột phần tử điều khiển. Nhờ vậy, những giá tr ị này có thể làm sáng tỏ về quan hệ thờ i gian: P phụ thuộc vào sai số hiện tại, I phụ thuộc vào tích lũy các sai số quá khứ, và D dự đoán các sai số tương lai, dựa vào tốc

độ thay đổi hiện tại. Sơ đồ điều khiển PID được đặt tên theo ba khâu hi ệu ch ỉnh c ủa nó, tổng c ủa ba khâu này tạo thành bở i các bi ến điều khiển (MV). Ta có:

trong đó:

28

CƠ SỞ LÝ THUYẾT Pout, I out, và Dout là các thành ph ần đầu ra từ ba khâu của bộ điều khiển PID, được xác đị nh

như dưới đây: 

Khâu tỉ lệ

Khâu tỉ lệ (đôi khi còn đượ c gọi là độ l ợ i) làm thay đổi giá tr ị đầu ra, tỉ lệ vớ i giá tr ị sai số hiện tại. Đáp ứng t ỉ lệ có thể được điều chỉnh b ằng cách nhân sai s ố đó vớ i một hằng s ố K p,

đượ c gọi là độ lợ i tỉ lệ. Khâu tỉ lệ đượ c cho bở i:

trong đó: Pout: thừa số tỉ lệ của đầu ra K p: Độ lợ i tỉ lệ, thông số điều chỉnh e: sai số = SP − PV t : thờ i gian hay thờ i gian tức thờ i (hiện tại)

Độ lợ i của khâu tỉ lệ lớn là do thay đổi lớ n ở đầu ra mà sai số thay đổi nhỏ. Nếu độ lợ i của khâu tỉ lệ quá cao, h ệ thống sẽ không ổn định. Ngượ c lại, độ lợ i nhỏ là do đáp ứng đầ u ra nhỏ trong khi sai s ố đầu vào lớ n, và làm cho b ộ điều khiển kém nhạy, hoặc đáp ứng chậm. Nếu độ lợ i của khâu tỉ lệ quá thấp, tác động điều khiển có thể sẽ quá bé khi đáp ứng vớ i các nhiễu của hệ thống. 

Khâu tích phân

Phân phối của khâu tích phân (đôi khi còn gọ i là reset ) tỉ lệ thuận vớ i c ả biên độ sai số lẫn quảng thờ i gian xảy ra sai số. Tổng sai s ố tức thờ i theo thờ i gian (tích phân sai s ố) cho ta tích

lũy bù đã đượ c hiệu ch ỉnh trước đó. Tích lũy sai số sau đó đượ c nhân với độ l ợ i tích phân và cộng v ớ i tín hiệu đầu ra của b ộ điều khiển. Biên độ phân phối của khâu tích phân trên t ất c ả

tác động điều chỉnh được xác đị nh bởi độ lợ i tích phân, K i. Thừa số tích phân đượ c cho bở i:

trong đó 29

CƠ SỞ LÝ THUYẾT I out: thừa số tích phân c ủa đầu ra K i: độ lợ i tích phân, 1 thông s ố điều chỉnh e: sai số = SP − PV t : thờ i gian hoặc thờ i gian tức thờ i (hiện tại)

τ: một biến tích phân trung gian Khâu tích phân (khi cộng thêm khâu t ỉ l ệ) s ẽ tăng tốc chuyển động c ủa quá trình t ới điểm

đặt và khử s ố dư sai số ổn định v ớ i m ột t ỉ lệ ch ỉ ph ụ thu ộc vào bộ điều khiển. Tuy nhiên, vì khâu tích phân là đáp ứng của sai số tích lũy trong quá khứ , nó có thể khiến giá tr ị hiện tại vọt lố qua giá tr ị đặt (ngang qua điểm đặt và tạo ra một độ lệch với các hướ ng khác). 

Khâu vi phân

Tốc độ thay đổi của sai số qua trình đượ c tính toán b ằng cách xác định độ dốc c ủa sai số theo thờ i gian (tức là đạo hàm bậc m ột theo thờ i gian) và nhân t ốc độ này với độ l ợ i t ỉ l ệ K d .

Biên độ c ủa phân phối khâu vi phân (đôi khi đượ c g ọi là t ốc độ) trên tất c ả các hành vi điều khiển đượ c giớ i hạn bởi độ lợ i vi phân, K d . Thừa số vi phân đượ c cho bở i:

trong đó Dout: thừa số vi phân của đầu ra K d : Độ lợ i vi phân, m ột thông số điều chỉnh e: Sai số = SP − PV t : thờ i gian hoặc thờ i gian tức thờ i (hiện tại)

Khâu vi phân làm ch ậm tốc độ thay đổi của đầu ra bộ điều khiển và đặc tính này là đang chú ý nhất để đạt t ới điểm đặt c ủa bộ điều khiển. T ừ đó, điều khiển vi phân đượ c s ử d ụng để làm giảm biên độ v ọt lố đượ c t ạo ra bở i thành phần tích phân và tăng cường độ ổn định c ủa bộ điều khiển hỗn hợ p. Tuy nhiên, phép vi phân c ủa một tín hiệu sẽ khuếch đại nhiễu và do đó khâu này s ẽ nh ạy hơn đối vớ i nhiễu trong sai số, và có th ể khi ến quá trình tr ở nên không ổn

định nếu nhiễu và độ lợi vi phân đủ lớ n.

30

CƠ SỞ LÝ THUYẾT 

Tóm lại:

Khâu tỉ lệ, tích phân, vi phân đượ c cộng lại với nhau để tính toán đầu ra của bộ điều khiển

PID. Định nghĩa rằ ng u(t ) là đầu ra của bộ điều khiển, biểu thức cuối cùng của giải thuật PID là:

Các phân tích v ề thiết k ế m ột b ộ điều khiển PID k ỹ thuật số trên một vi điều khiển (MCU) hoặc thiết bị FPGA yêu cầu dạng chuẩn của bộ điều khiển PID phải đượ c r ờ i r ạc hóa. Vi phân bậc m ột được xác định b ằng sai phân h ữu h ạn lùi. Khâu tích phân đượ c r ờ i r ạc hóa, vớ i thờ i gian lấy mẫu Δt như sau,

Khâu vi phân được xác đị nh bở i,

Do đó, mộ t giải thuật vận t ố c cho việc thực thi bộ điều khiển PID r ời r ạc trên một MCU đạt đượ c bằng cách đạo hàm u(t ), sử dụng các số xác định từ đạo hàm bậc một và đạo hàm bậc hai, tìm ra u(t k) cuối cùng ta đượ c:

Hay: u (tk ) = u( t k −1 ) + ( K p u ( tk )

=

+

K i .∆t

2

+

Kd ∆t

).e(tk ) + ( − K p

−

2. K d

u (tk −1 ) + a0 .e (tk ) + a1 .e (tk −1 ) + a 2 .e (tk − 2 )

31

∆t

).e( tk −1 ) +

Kd ∆t

.e( tk − 2 )


Hình 3.23 Bộ điề u khiể n PID 3.5.4 M ạch cầu H: 

Giớ i thiệu:

Mạch cầu H là mạch biến đổi điện áp một chiều. Đầu vào của mạch gồm điện áp DC dùng cung cấp cho động cơ và tín hiệu điề u khiển hướng và độ r ộng xung. Đầ u ra của mạch là điện

áp DC đã được thay đổi tương ứ ng vớ i tín hiệu độ r ộng xung đầ u vào. Lý do ph ải sử dụng mạch cầu H để điều khiển động cơ DC vì tín hiệ u PWM từ vi điều khiển cấp ra thay đổi từ 05V và dòng cung c ấ p tối đa là 8mA, trong khi điện áp dùng cho động cơ DC là từ 0 – 12V và

dòng lên đế n tối đa 2A. Vì vậ y không th ể sử dụng vi điều khiển để tr ực tiếp điều khiển vận tốc động cơ DC đượ c. Có nhiều cách thi ết k ế mạch cầu H nhưng nói chung đều đượ c thiết k ế sử dụng BJT, MOSFET hoặc Relay. Sơ đồ sau ch ỉ là dạng cơ bản đưa ra ở đây để làm rõ ph ần giải thích nguyên tắc ho ạt động ở dướ i. Trên thực t ế, có thể thay thế các BJT b ằng 1 trong 3 lo ại nói ở

trên, kèm theo đó là mạ ch kích mở ứng vớ i các loại khác nhau. Sơ đồ thiết k ế cơ bản:

Hình 3.24 Thiế t k ế cơ bản của mạch cầu H

32

CƠ SỞ LÝ THUYẾT 

Nguyên tắc hoạt động:

Mạch cầu H được điều khiển vào 2 ch ế độ: 

Chế độ thuận: Giả sử Q1 và Q4 cùng đóng, Q2 và Q3 hở mạch. Khi đó ta có:

Hình 3.25 M ạch cầu H chế độ thuận

Khi đó điện áp đặt trên động cơ là dương (chân 1), âm (chân 2). Động cơ quay thuậ n. 

Chế độ nghịch: Giả sử Q2 và Q3 cùng đóng, Q1 và Q4 hở mạch. Khi đó ta có:

Hình 3.26 M ạch cầu H chế độ nghịch

Khi đó điện áp đặt trên động cơ là dương (chân 2), âm (chân 1). Động cơ quay nghị ch.

33

CƠ SỞ LÝ THUYẾT Như vậy ta thấy thông qua mạch cầu H ta có th ể điều khiển đượ c chiều và vận tốc của động cơ (bằng cách thay đổ i giá tr ị điện áp đặt vào động cơ). Tuy nhiên có m ột chú ý quan tr ọng khi điều khiển cầu H đó là hiện tượ ng trùng dẫn. −

Hiện tượ ng trùng dẫn:

Hiện tượ ng trùng dẫn x ảy ra khi 2 khóa trên 1 nhánh đóng cùng lúc , ví dụ Q1 và Q2 cùng

đóng, khi đó sẽ xảy ra hiện tượ ng ngắn mạch nguồn. Vì vậy chú ý khi điề u khiển mạch cầu H là phải có deadband gi ữa 2 tín hiệu điều khiển trên cùng 1 nhánh. Deadband này ta có th ể tạo ra khi lập trình điều khiển cầu H (deadband mềm) hoặc sử dụng một số linh kiện thụ động tạo ra thờ i gian tr ễ (deadband c ứng). Trong luận văn có sử d ụng m ạch c ầu H dùng MOSFET và kích MOSFET sử dụng IC IR2184 của hãng International Rectifier . Đây là IC kích FET chuyên dụng và có deadband c ứng.

Hình 3.27 Sơ đồ cấ u t ạo của IR2184 −

Tần số hoạt động:

Một chú ý khác khi s ử d ụng mạch c ầu H điều khiển v ận t ốc động cơ đó là tầ n số đóng cắt các khóa trên m ạch cầu H. Nếu tần số đóng cắt quá nhỏ thì động cơ sẽ hoạt động không êm ái do các hài t ần số cao gây nên dao động trên cuộn dây động cơ phát ra tiếng độ ng. Nếu t ần số

đóng cắt quá cao thì có th ể MOSFET không đáp ứng đượ c ho ặc m ạch kích MOSFET không tốt làm thờ i gian lên/xuống c ủa chân G (tương ứng v ớ i thời gian đóng/cắt MOSFET) lớ n có thể gây ra hiện tượ ng trùng dẫn. Thông thườ ng vớ i ứng dụng điều khiển động cơ, chúng ta chọn tần số đóng cắt MOSFET là 20 kHz. 34


3.6 Điều khiển robot bám theo line: Để điều khiển robot bám theo line ta s ử dụng một mạch cảm biến gọi là mạch dò line. Mạch dò line gồm 6 led cùng v ớ i 6 quang tr ở sẽ giúp ta phát hi ện đượ c nền đen và vạch tr ắng. Xử lý thông tin này ta sẽ bi ết đượ c vị trí hiện t ại c ủa robot là đang ở gi ữa line, lệch bên phải hoặc lệch bên trái so v ớ i giữa line. Từ thông tin trên ta s ẽ phát xung điều khiển vận tốc 2 bánh xe cho phù hợ p. 3.6.1 Nguyên t ắ c phân bi ệt line đen và trắ ng:

Để phát hiện line đen và trắng ta sử dụng mạch dò line. Mạch dò line gồm 2 thành phần chính là quang tr ở và led. 

Quang trở :

Quang tr ở là linh kiện mà giá tr ị điện tr ở của nó sẽ thay đổi theo cường độ ánh sáng chi ếu vào. Khi chi ếu ánh sáng m ạnh vào quang tr ở thì giá tr ị điện tr ở của nó sẽ giảm. Khi đặt quang tr ở trong bóng t ối thì giá tr ị điện tr ở của nó sẽ tăng. Giá trị điện tr ở của quang tr ở đượ c sử dụng trong luận văn này thay đổ i từ 2 – 90 kOhm.

Hình 3.28 Quang tr ở

Ứ ng dụng đặc tính này c ủa quang tr ở, ta có th ể thiết k ế một mạch có tác d ụng giúp vi điề u khiển nhận biết được cường độ ánh sáng chi ếu vào quang tr ở như sau:

35


Hình 3.29 M ạch ứ ng d ụng quang tr ở

Giả s ử cường độ ánh sáng chi ếu vào quang tr ở thay đổi làm giá tr ị điện tr ở c ủa quang tr ở biến thiên từ 2 – 20 kOhm và VCC = 5 Vdc. Nếu điện tr ở của quang tr ở là 2 kOhm:  = 2�1 0 + 2 ∗ 5() = 0.83 () Nếu điện tr ở của quang tr ở là 20 kOhm:  = 20�10 + 20 ∗ 5() = 3.3 ()

Như vậy ta thấy Vout sẽ thay đổi từ Vmin = 0.83 – Vmax = 3.33 V. S ử dụng bộ ADC trong vi điều khiển ta sẽ bi ết đượ c khi nào ánh sáng chi ếu vào quang tr ở m ạnh, khi nào không có ánh sáng chiếu vào quang tr ở . 

Cơ chế phân bi ệt line đen và trắng:

Theo cơ chế phản xạ ánh sáng thì màu tr ắng sẽ phản xạ ánh sáng nhi ều hơn màu đen, nghĩa là nếu chiếu ánh sáng vào line màu tr ắng ta sẽ thấy chói mắt hơn chiếu ánh sáng vào màu đen. Sử d ụng cơ chế này, nếu ta đặt một quang tr ở k ế bên một led vớ i nhiệm v ụ của quang tr ở là nhận biết cường độ ánh sáng c ủa led phản xạ trên mặt đất. Như vậy nếu led chiếu vào line tr ắng thì ánh sáng s ẽ phản xạ vào quang tr ở nhiều làm giá tr ị điện tr ở của quang tr ở giảm, khi

đó V out gi ảm. Cũng vậ y nếu led chiếu vào nền đen thì ánh sáng phả n x ạ vào quang tr ở ít làm giá tr ị điện tr ở của quang tr ở tăng và Vout tăng.

36


Hình 3.30 Cách s ắ p xế p quang tr ở và Led trên m ạch dò line

Một điểm c ần chú ý là vì Vout thay đổi liên tục t ừ 0.83 – 3.33 V nên n ếu ta chỉ xét 2 m ốc

điện áp là 0.83 V và 3.33 V thôi thì n ếu đặt robot vào đườ ng biên của line tr ắng, mức điện áp Vout khi đó sẽ là ở khoảng giữa 0.83 và 3.33V (do ch ỉ một phần ánh sáng trong line tr ắng phản chiếu lại quang tr ở, một phần ánh sáng còn l ại ở ngoài nền đen sẽ không phản chiếu lại quang tr ở nên giá tr ị điện tr ở quang tr ở lúc này k hông đạ t mức tối đa hoặc tối thiểu), robot sẽ không nhận bi ết được là nó đang ở line tr ắng hay đã ra nền đen. Để giải quyết v ấn đề này, ta sẽ l ấy trung bình cộng 2 giá tr ị điện áp lúc ở line tr ắng và lúc ở nền đen  = ( + min )⁄2

làm ngưỡ ng xét line. Tại một thời điểm nếu giá tr ị Vout lớn hơn VTH thì robot đang ở nền đen, còn nếu Vout nhỏ hơn VTH thì robot đang ở line tr ắng. Tuy nhiên n ếu đặt ngưỡ ng VTH làm mức xét line thì giá tr ị ADC không ổn định (giá tr ị

ADC thay đổ i trong khoảng nhỏ) ở g ần m ức ngưỡ ng VTH s ẽ khi ến robot có thể xác định line sai. Để giải quyết tình tr ạng này ta đặt một giá tr ị ∆ và quy đị nh mức ngưỡ ng của line tr ắng là

 =  − ∆ , m ức ngưỡ ng của line đen là  =  + ∆. Khi đó robot sẽ ở line tr ắng nếu  ≤  , robot ở n ền đen nếu  ≥  . Nếu trườ ng hợ p  <  <  thì robot sẽ sử dụng giá tr ị VOUT cũ để xét line. 3.6.2 Nguyên t ắc điề u khiể n robot bám theo line:

Giả sử mạch dò line gồm 4 cảm biến quang tr ở. Robot đượ c cho là ch ạy đúng line (chạy giữa line) nếu nó bắt đượ c line tr ắng bằng 2 led gi ữa (led 2 và 3).

Hình 3.31 M ạch dò line s ử d ụng 4 cảm biế n

37


Hình 3.32 Trườ ng hợ p robot chạ y giữ a line

Giả sử robot bắt đượ c line bằng 2 cảm bi ến bên trái (cảm biến 1 và 2), ta gọi là robot chạy lệch phải. Với trườ ng hợ p này, ta c ần phải làm robot quẹo một chút sang bên trái để line nằm giữa 2 c ảm biến 2 và 3, nghĩa là ta cầ n phải điều khiển vận tốc bánh ph ải nhanh hơn bánh trái một chút.

Hình 3.33 Trườ ng hợ p robot chạ y lệch phải

Tương tự như vậy nếu robot bắt đượ c line bằng 2 cảm bi ến bên phải (c ảm bi ến 3 và 4), ta gọi là robot chạy l ệch trái. Với trườ ng h ợ p này, ta phải điều khiển sao cho vận tốc bánh trái

nhanh hơn vận tốc bánh phải một chút đến khi robot bắt đượ c line tr ắng bằng 2 cảm biến giữa giữa.

38


Hình 3.34 Trườ ng hợ p robot chạ y lệch trái

Câu hỏi đặt ra ở đây là ‘một chút’ đó là bao nhiêu và cái gì quyết đị nh cái ‘một chút’ đó. Ta có thể s ử d ụng giải thuật điều khiển m ờ để quyết định cái giá tr ị ‘một chút’ đó tuy nhiên trong bài luận văn này sẽ áp dụng thuật toán PID. 3.6.3 Áp d ụng thuật toán PID vào dò line:

Nếu ta giả sử một biến sai số e thay đổi như sau: −

Nếu robot chạy giữa line thì e = 0.

−

Nếu robot chạy lệch phải thì e = -1.

−

Nếu robot chạy lệch trái thì e = 1.

Như ta đã biết thuật toán PID có đầu ra thay đổ i sao cho tín hi ệu hồi tiế p về có sai s ố bằng 0 so với đầu vào. Vậy nếu sai số đầu vào ta quy định đó là biế n e ở trên và đầu ra là m ột biến ∆V

chỉ giá tr ị vận tốc tăng thêm đố i vớ i bánh trái và gi ảm đi đối vớ i bánh phải thì cứ sau 1

chu k ỳ tính PID này ta có v ận t ốc 2 bánh xe thay đổ i sao cho cu ối cùng xác l ậ p khi robot bắt

đượ c line tr ắng bằng 2 cảm biến 2 và 3. Bi ến ∆V là một biến có dấu và t ỷ lệ vớ i e nên: −

Giả s ử robot chạy lệch phải thì ∆V < 0 nên v ận t ốc bánh trái gi ảm và vận t ốc bánh

phải tăng khiến robot có xu hướ ng chạy lệch sang trái. −

Giả sử robot chạy l ệch trái thì ∆V > 0 nên vận tốc bánh trái tăng và vận tốc bánh

phải giảm khiến robot có xu hướ ng chạy lệch sang ph ải.

39

THIẾT K Ế VÀ THI CÔNG

CHƯƠNG 4

THIẾT KẾ VÀ THI CÔNG

4.1 Mạch điện tử : Trong luận văn này sử dụng các module m ạch sau: −

Mạch PIF 5R31 Development Kit dùng làm b ộ xử lý trung tâm robot slave.

−

Mạch Stamp slave dùng liên k ết các module m ạch trên robot slave.

−

Mạch dò line cho robot slave.

−

Mạch cầu H làm mạch công su ất điều khiển các động cơ trên robot slave.

−

Mạch reader RFID ACE630 dùng đọ c các thẻ (Tag) RFID.

−

Mạch TI eZ430-RF2500 dùng truy ền nhận thông qua sóng RF.

−

Mạch TI EKS-LM4F232 dùng làm b ộ x ử lý trung tâm điề u phối ho ạt động của các

robot slave. Trong các m ạch trên thì mạch dò line, mạch c ầu H và m ạch Stamp slave là do tôi t ự thi ết k ế và thi công. 4.1.1 M ạch dò line:

Mạch dò line đượ c thiết k ế gồm 6 led và 6 c ảm biến quang tr ở cùng 1 ngu ồn 3.3VDC chung mass v ớ i nguồn vi điều khiển.

40


Hình 4.1 Sơ đồ nguyên lý m ạch dò line

Hình 4.2 Hình thự c t ế mạch dò line

41

THIẾT K Ế VÀ THI CÔNG 4.1.2 M ạch cầu H:

Mạch cầu H sử dụng 4 FET IRF540N và IC kích FET IR2184.

42


Hình 4.3 Sơ đồ nguyên lý m ạch cầu H

Hình 4.4 Hình thự c t ế mạch cầu H

Mạch cầu H thiết k ế như trên sử dụng tất cả 3 giá tr ị nguồn DC: 

5V dùng chạy IC 4001 (cổng NOR).



12V dùng ch ạy IC kích FET IR2184.



12V/24V dùng ch ạy động cơ.

Ưu điểm c ủa m ạch c ầu H này đó là chỉ c ần c ấ p m ột nguồn duy nh ất để ch ạy động cơ cho toàn bộ mạch. Tuy nhiên do động cơ có thể chạy nguồn 24V hoặc 12V nên có thi ết k ế một

jumper để nối tắt IC nguồn 12V 7812. Khi s ử dụng nếu nguồn chạy động cơ là 24V thì bỏ (không nối) jumper này. Nếu nguồn chạy động cơ là 12V thì phả i nối jumper này.

43

THIẾT K Ế VÀ THI CÔNG Một thiếu sót của mạch cầu H này đó là cầu chì. Thườ ng mạch công su ất phải có cầu chì để bảo vệ ngắn mạch, tuy nhiên khi thi ết k ế, do yêu cầu về kích thước nên tôi đã bỏ qua ph ần bảo vệ này. Thực tế thử nghiệm mạch cầu H này cho k ết quả r ất tốt: 

Điều xung điề u khiển động cơ tần số 20 kHz mạch đáp ứng tốt.



Đổi chiều động cơ liên tục và tức thờ i mạch đáp ứng tốt, cả b ốn FET đều không bị

nóng. 4.1.3 M ạch Stamp slave:

Mạch stamp slave c ắm lên trên mạch PIF 5R31 dùng k ết nối các module mạch cầu H, mạch reader RFID, mạch eZ430-RF2500 và encoder c ủa 2 động cơ lạ i vớ i nhau.

44


Hình 4.5 Sơ đồ khố i mạch Stamp slave

45


Hình 4.6 Hình thự c t ế mạch stamp và k ết nố i vớ i board MCU slave

Trên mạch Stamp slave này có thi ết k ế m ột khối nguồn riêng cho nguồn 5VDC dùng cho encoder và các module khác mà không s ử dụng nguồn 5V chung v ớ i nguồn của vi điều khiển LM3S5R31 nhằm mục đích tránh gây nhiễu đáng tiếc lên vi điề u khiển.

4.2 Thiết kế và thi công cơ khí: Như mục tiêu của đề tài, ta cần một hệ thống robot vận chuyển hàng ch ịu sự điều khiển của một master. Trong khuôn kh ổ của luận văn, mô hình cơ khí gồ m có 2 r obot đóng vai trò là

Slave dò đườ ng theo nguyên t ắc dò line và một hệ thống sân mô ph ỏng theo thực tế. 4.2.1 Thiế t k ế và thi công sàn làm vi ệc của robot:

Vớ i yêu cầu các robot di chuyển theo line và có kh ả năng tránh nhau (không va chạ m khi chạy cùng line ho ặc t ại giao lộ (những điểm 2 line giao nhau)), sàn làm vi ệc c ủa robot đượ c thiết k ế như sau: −

Robot sẽ tránh nhau b ằng cách d ừng lại, nghĩa là tại một giao lộ nếu robot A đến

trước và đang qua giao lộ thì robot B đến giao l ộ sau ph ải dừng l ại. Sau khi robot A đ i qua khỏi giao lộ thì robot B s ẽ đượ c phép tiế p t ục di chuyển. Để dừng lại thì ta phải cung cấ p cho 46

THIẾT K Ế VÀ THI CÔNG robot một dấu hiệu báo đến điểm dừng, ở đây ta sử dụng là 1 line nhỏ nằm trướ c giao lộ. Mặt khác cần một dấu hiệu nào đó để các robot có th ể tự định vị vị trí của nó, ở đây ta cũng sử dụng line để robot tự định vị nó. Như vậ y cứ mỗi điểm định vị của robot sẽ có 1 cặ p line song

song. 1 line để định v ị trí cho robot và 1 line để cung cấ p dấu hi ệu cho robot dừng lại n ếu nó nhận đượ c lệnh dừng lại. −

Chia sàn làm vi ệc ra nhiều phần nh ỏ gọi là block. Mỗi block được đánh số thứ tự

riêng và có t ối đa 2 kho hàng trên 1 block. −

Trong 1 block s ẽ có nhiều điểm để robot định vị trí của nó gọi là point. Mỗi một

point sẽ có 1 cặ p line ngắn nằm vuông góc v ới đườ ng line robot bám theo. −

Để tránh hiện tượng đối đầu giữa các robot, quy đị nh m ỗi đườ ng line chỉ cho phép

robot di chuyển theo 1 hướ ng nhất định. Như vậ y mỗi kho hàng sẽ có 1 đường line đi vào và 1

đường line đi ra. −

−

Phân các đườ ng line cho robot di chuy ển bám theo ra thành 2 lo ại: o

Đường chính: Đườ ng line nối các block l ại vớ i nhau.

o

Đườ ng phụ: Đườ ng line nối từ đường chính đến các kho và ngượ c lại.

Có thể có nhiều đường chính nhưng quy đị nh một kho chỉ có 1 đườ ng phụ đi vào và

1 đườ ng phụ đi ra. Trong khuôn kh ổ lu ận văn, sàn làm việc c ủa robot chỉ có 1 c ặp đường chính (1 đường để robot di chuyển từ phải sang trái, 1 đườ ng robot di chuyển từ trái sang ph ải).

Hình 4.7 Bản thiế t k ế một block −

Để robot định vị đượ c vị trí của nó thì ta đánh dấu các point như sau: 47


Hình 4.8 S ố thứ t ự các point trong 1 block −

Để robot có thể dừng lại ở các vạch báo dừng và cho phép robot ở hướng đối đầu đi

qua giao lộ mà ko có va ch ạm gì thì khoảng cách t ừ các vạch báo dừng phải đủ lớ n, ít nhất

cũng phải b ằng 1 nửa chiều ngang của robot. Vớ i mô hình ở bài luận văn này, tôi quyế t định lựa chọn khoảng cách này là 20 cm. −

Khoảng cách t ừ line định vị đến line báo dừng cũng phả i lựa chọn sao cho robot có

đủ thờ i gian xử lý (gửi bản tin báo vị trí về master và master phải đủ thờ i gian xử lý để báo xung đột về cho robot slave). V ớ i mô hình ở bài luận văn này, tôi quyết đị nh lựa chọn khoảng cách này là 15 cm. −

−

Thống nhất các kích thướ c lại ta có: o

Khoảng cách t ừ line di chuyển đến line báo d ừng là 20 cm.

o

Khoảng cách giữa line bao vị trí và line báo dừng là 15 cm.

Vớ i mô hình lu ận văn, ta xây dựng sân robot v ớ i 6 kho hàng, gồm 1 kho tổng (kho

lấy hàng) và 5 kho ph ụ (kho nhận hàng) đượ c chia ra thành 3 block.

48


Hình 4.9 Kích thướ c chi tiế t sàn làm vi ệc của robot

Hình 4.10 V ị trí các point trên sàn làm vi ệc của robot

49

THIẾT K Ế VÀ THI CÔNG 4.2.2 Thi công mô hình robot:

Mô hình robot 3 bánh v ới 2 sau điều khiển đượ c. Hai bánh sau m ỗi bánh được điều khiển bởi 1 động cơ DC thông qua 1 bộ gán bánh xe. Động cơ DC có gắ n sẵn encoder 300 xung.

Bánh trước là bánh omni không điều khiển. Robot sử dụng nguồn pin Lipo 3 cell 11.1V. Ở phần đầu robot có nơi để gắn mạch dò line. Ở giữa thân robot có nơi để gắn pin Lipo 3 cell.

Hình 4.11 Mô hình cơ khí robot slave

Mô hình robot slave hoàn ch ỉnh:

Hình 4.12 Mô hình robot slave hoàn ch ỉ nh

50


4.3 Xây dự ng giải thuật giao tiếp trong mạng không dây: 4.3.1 Các quy t ắc giao tiế p trong mạng:

Các quy t ắc cần có trong giao ti ế p mạng trong hệ thống: −

Đăng ký: Một khi robot slave đượ c khởi động nó cần phải gửi một frame lên master

để đăng ký sự hiện diện của nó. −

Kiểm tra frame truyền có đến đối tượ ng cần nh ận không bằng cách c ứ g ửi 1 frame

thì chờ nhận 1 frame báo đã nhận được frame đó. Nế u không nh ận được frame báo đã nhận thì bên gửi sẽ gửi lại bản tin đó mộ t lần nữa. 4.3.2 C ấu trúc frame truyề n trong mạng:

Cầu trúc frame truyền RF trong mạng gồm 12 Bytes c ố định bao gồm: −

1 Byte đầu tiên chỉ số byte trong frame truyền (trong luận văn này cố định đây là số

−

3 byte tiếp theo là địa chỉ của nơi gửi.

−

3 byte tiếp theo là địa chỉ của nơi nhận.

−

5 byte dữ liệu sau cùng g ồm: 1 byte đầu tiên xác đị nh mục đích của frame và 4 byte

12).

còn lại là dữ liệu phục vụ cho mục đích của frame. 1 Byte

chỉ số Byte trong frame

3 Bytes địa chỉ Sender

3 Bytes địa chỉ Receiver

1 Byte xác

định

4 Bytes dữ liệu

mode

Hình 4.13 C ấu trúc frame hoàn ch ỉ nh 

Nhữ ng frame truyền dành cho robot slave:

Địa chỉ sender: địa chỉ robot slave. Địa chỉ receiver: địa chỉ master. 

Frame robot slave truyền lên master để đăng ký sự hiện diện của nó trong m ạng:

1 Byte




51

1 Byte ‘g’

4 Bytes bỏ (0000)

THIẾT K Ế VÀ THI CÔNG Hình 4.14 Frame slave truy ền để đăng ký sự hiện diện 

Frame robot slave truyền vị trí hiện tại của nó về cho master:

1 Byte




1 Byte ‘p’

1 Byte

1 Byte

1 Byte

1 Byte

chỉ

chỉ

bỏ

Block

Point

(0)

hướng

Hình 4.15 Frame slave truy ề n vị trí hiện t ại của nó 

Frame slave truyền mã số của gói hàng lên master:

1 Byte




1 Byte ‘r’

4 Bytes ID của Tag

Hình 4.16 Frame slave truy ề n mã số gói hàng 

Frame slave tr ả lờ i xác nhận việc đã nhận một frame từ master:

1 Byte




1 Byte ‘o’

4 Bytes bỏ (0000)

Hình 4.17 Frame slave tr ả lờ i xác nh ận việc đã nhận frame thành công 

Nhữ ng frame truyền dành cho master:

Địa chỉ sender: địa chỉ master. Địa chỉ receiver: địa chỉ robot slave. 

Frame master truyền để kiểm tra sự tồn tại của slave trong m ạng:

1 Byte




1 Byte ‘c’

4 Bytes bỏ (0000)

Hình 4.18 Frame master truy ề n kiể m tra sự t ồn t ại của slave 

Frame master truyền để yêu cầu slave phải dừng lại để nhường đườ ng cho slave

khác, đồng thời cũng là frame master truyền để cho phép slave ti ế p tục chạy tr ở lại:

52

THIẾT K Ế VÀ THI CÔNG 1 Byte chỉ số Byte trong frame



4 Bytes bỏ (0000)

1 Byte ‘s’

Hình 4.19 Frame master truy ề n yêu c ầu slave d ừ ng/chạ y 

Frame master truyền địa ch ỉ kho của gói hàng do slave yêu c ầu (nghĩa là khi slave

gửi mã số gói hàng đến thì master sẽ tr ả lờ i bằng frame này): 1 Byte chỉ 1 Byte chỉ

1 Byte chỉ

số Byte

1 Byte chỉ 1 Byte chỉ 3 Bytes địa chỉ Sender

trong frame


1 Byte ‘d’

Point tại Block của Point của đó Robot khối hàng khối hàng sẽ nhập

Point tại đó Robot sẽ rời

HighWay

HighWay

Hình 4.20 Frame master truy ền địa chỉ kho của gói hàng

Frame trên còn có 2 byte cu ối chỉ point tại đó robot sẽ nh ập vào đườ ng chính và r ời khỏi

đường chính để đến kho hàng. Hai byte này dùng cho trườ ng h ợ p sàn làm vi ệc c ủa robot có nhiều hơn 1 cặp đườ ng chính. Trong lu ận văn này do sàn làm việ c của robot chỉ có 1 cặ p

đường chính (2 đườ ng chính) nên 2 byte này c ố định. 

Frame master tr ả lờ i xác nhận việc đã nhận một frame từ slave:

1 Byte




1 Byte ‘o’

4 Bytes bỏ (0000)

Hình 4.21 Frame master truy ề n xác nh ận việc nhận 1 frame thành công

4.4 Giải thuật chương trình vi điề u khiển: 4.4.1 Giải thuật chương trình MCU trên module eZ430 -RF2500:

MCU MSP430F2274 trên module này có tác d ụng truyền nhận vớ i IC CC2500 qua giao tiế p SPI và giao tiế p vớ i một MCU khác thông qua chu ẩn UART. Đố i vớ i luận văn này, nhiệm v ụ của MCU này là nh ận bản tin từ CC2500, kiểm tra địa chỉ đến của bản tin có trùng với địa chỉ của board không. N ếu đúng đị a chỉ thì nó s ẽ truyền dữ liệu nhận đượ c (bỏ đi 4 byte gồm byte chỉ số byte trong frame nh ận và 3 byte đị a chỉ receiver) cho MCU khác qua USCI0_UART mode.

53



1 Byte xác

4 Bytes dữ liệu

định mode

Hình 4.22 Frame truy ề n t ừ module eZ430- RF2500 đế n master/slave

Module eZ430-RF2500 đượ c lậ p trình có khả năng thay đổi đị a chỉ sender dễ dàng. Khi

đượ c khởi động, module này sẽ chờ nhận 3 byte t ừ master/slave dùng làm địa chỉ sender. Sau khi nhận được địa chỉ sender, module này m ớ i bắt đầu nhận frame truyền như sau: 1 Byte 3 Bytes địa chỉ Receiver

chỉ

4 Bytes dữ liệu

mode

Hình 4.23 Frame truy ề n t ừ master/slave đế n module eZ430-RF2500

Chương trình chính: void main()

Chương trình ngắt nhận từ CC2500:

Start

Ngắt nhận frame hoàn chỉnh từ CC2500

Khởi tạo USCIA0_ UARTMode

Start

Khởi tạo USCIB0_SPIMode

Kiểm tra xem 3 byte địa chỉ receiver của frame vừa nhận có trùng với địa chỉ của board không

Vào chế độ tiết kiệm năng lượng mức thứ 4 (low power mode 4)

NO

End

End

54

YES

Tạo frame truyền lên master/slave qua USCIA0


Chương trình chính có nhiệ m vụ khở i tạo module SPI dùng giao ti ế p CC2500 và module USCIA0 chế độ UART. Sau khi kh ở i tạo xong 2 module thì MCU s ẽ vô chế độ tiết kiệm năng

lượ ng mức thứ 4. Trong chế độ này, MCU ch ỉ làm việc khi có ngắt xày ra.

55


Chương trình ngắt nhận USCIA0: Ngắt nhận USCIA0

Start

Khởi tạo chạy 1 lần : Source_index = 0; SystemStatus = ReceiveSource

SystemStatus?

ReceiveSource

Mỗi lần vào ngắt thì nhận 1 byte cho đến khi nhận đủ 3 byte do master/slave gửi xuống làm địa chỉ sender

Nhận đủ 3 byte?

Run Mỗi lần vào ngắt nhận 1 byte cho đến khi đủ 8 byte gồm 3 byte địa chỉ receiver và 5 byte dữ liệu NO

Nhận đủ 8 byte?

YES

Tạo thành frame hoàn chỉnh truyền qua CC2500

NO

End

56

YES

SystemStatus = Run

THIẾT K Ế VÀ THI CÔNG 4.4.2 Giải thuật chương trình trên vi điề u khiể n của Master:

Hình 4.24 Mô hình giao ti ế p giữ a master vớ i các module

Giải thuật trên master sử dụng các module sau:

Hình 4.25 Các module s ử d ụng trên vi điề u khiể n của master 

Cấu trúc các b ản tin giao ti ế p mạng của master:

57

THIẾT K Ế VÀ THI CÔNG Khi nhận đượ c yêu c ầu c ủa slave là đăng ký sự hiện di ện, master s ẽ tr ả về bản tin có cấu

trúc như sau để xác nh ận đã nhận yêu cầu từ slave:


1 Byte ‘o’

4 Bytes bỏ (0000)

Khi nhận đượ c yêu cầu c ủa slave h ỏi v ị trí kho cho gói hàng, master s ẽ tr ả l ờ i b ằng frame có cấu trúc như sau: 1 Byte chỉ 1 Byte chỉ 1 Byte chỉ 1 Byte chỉ


Point tại 1 Byte ‘d’ Block của Point của đó Robot khối hàng khối hàng sẽ nhập

Point tại đó Robot sẽ rời

HighWay

HighWay

Khi cần yêu cầu một robot dừng lại, master sẽ gửi một bản tin có cấu trúc sau:


1 Byte ‘s’

4 Bytes bỏ (0000)



Sơ lượ c về các mảng đượ c sử dụng trong chương trình master:

−

Mảng hai chiều RobotID (RobotAddress) ch ứa địa chỉ của các robot đã đăng ký

trong mạng. Chiều thứ nhất chỉ mã số robotID. Chiều thứ hai gồm 3 byte chỉ địa chỉ truyền RF của robotID ở chiều thứ nhất. −

Mảng 1 chiều CollisionMap vớ i n ội dung của v ị trí thứ i trong mảng là point xung

đột vớ i point thứ i. Nếu point thứ i không có point xung độ t thì giá tr ị tại vị trí i trong mảng là (số point trong 1 block + 1). −

Mảng 2 chiều CollisionPoint chứa vị trí block và vị trí point xảy ra xung đột vớ i vị

trí hiện tại của các robot đã đăng ký trong mạ ng. Chiều thứ nhất chỉ mã số robotID. Chiều thứ hai gồm 2 byte ch ỉ vị trí block và vị trí point xảy ra xung độ t v ớ i vị trí hiện tại của robotID ở chiều thứ nhất. −

Mảng một chiều StoppedRobot ch ứa mã số robotID của các robot hi ện đang bị

dừng. 58

THIẾT K Ế VÀ THI CÔNG −

Mảng hai chi ều ProcessPoint ch ứa v ị trí block và vị trí point hiện t ại c ủa các robot

đã đăng ký. Chiể u thứ nhất chỉ mã s ố robotID. Chiều thứ hai gồm 2 byte chỉ vị trí block và vị trí point hiện tại của robot mang mã s ố robotID. −

Mảng một chiều ProcessingID ch ứa ID của những robot đã gử i dữ liệu vị trí đến

master chờ xử lý. Mảng này như mộ t stack chứa ID các robot đang chờ đượ c xử lý dữ liệu, tối

đa bằng 10. −

Mảng một chiều RegisteredID ch ứa ID của những robot đã đăng ký trong mạ ng.

Biến RegisteredNumber ch ỉ số lượng robot đã đăng ký trong mạ ng.

59


Chương trình chính: void main(void)

Chương trình ngắt UART0:

60


CheckPosition (void)

Start

Tăng ProcessIndex Lấy robotID thứ

lên 1.

ProcessIndex tối đa bằng 10

ProcessIndex trong

mảng ProcessingID

Lấy vị trí hiện tại của robotID (chứa trong mảng ProcessPoint) so sánh với các vị trí có trong mảng

Trùng

Ra lệnh cho robot mang số robotID dừng lại

CollisionPoint

Không trùng

Tìm vị trí point xung đột với vị trí hiện tại của robot và thêm vào mảng CollisionPoint

Thêm CurrentBlock và CollisionMap[Curr entPoint] vào vị trí

thứ robotID của mảng CollisionPoint

Kiểm tra xung đột của các robot bị dừng với các vị trí xung đột hiện tại

End

Chương trình ngắt UART6: 61

Lưu robotID vào mảng StoppedRobot


4.4.3 Giải thuật chương trình trên vi điề u khiể n của Slave:

Giải thuật chương trình trên robot slave sử dụng các module sau:

62


Module QEI: 2 bộ dùng đọc encoder 2 động cơ

Module PWM: 2 kênh điều khiển 2 động cơ

Module UART0: giao tiếp

máy tính vẽ vận tốc động cơ theo thời gian Module UART0: giao tiếp

với module RFID reader nhận ID của gói hàng Module UART2: giao tiếp

với module

LM3S5R31

eZ430_RF2500

ngắt 5ms chạy PID dùng dò line

Module Timer0:

Module Timer1: ngắt 10ms dùng kiểm tra và đọc

trạng thái của nút nhấn

Module ADC0: dùng đọc giá trị từ 6 cảm biến quang

Module System Timer: ngắt 1ms dùng làm hàm delay

trở để xác định line

Hình 4.26 Các module s ử d ụng trên vi điề u khiể n của robot slave 

Cấu trúc các b ản tin giao ti ế p mạng của robot slave:

Khi robot slave cần đăng ký sự hiện diện vớ i master thì sử dụng bản tin có cấu trúc sau:


1 Byte ‘g

4 Bytes bỏ (0000)

63

THIẾT K Ế VÀ THI CÔNG Khi robot slave cần gửi vị trí lên master thì sử dụng bản tin có cấu trúc sau:


1 Byte ‘p’

1 Byte

hướng

1 Byte

1 Byte

1 Byte

chỉ

chỉ

bỏ

Block

Point

(0)

Khi robot slave muốn thông báo v ớ i máy chủ sự tồn tại của mình (khi đượ c master h ỏi kiểm tra) thì sử dụng bản tin có cấu trúc sau:


1 Byte ‘a’

4 Bytes bỏ (0000)

Khi robot slave cần tr ả l ờ i bản tin yêu cầu dừng c ủa master thì sử dụng b ản tin có cấu trúc sau:




1 Byte ‘o’

4 Bytes bỏ (0000)

Lưu đồ giải thuật các chương trình trên robot slave:

Chương trình chính: void main(void) Start

Khởi tạo

Run Mode

Chạy chương trình RunModeProcess()

InitSystem()

System Status?

Study Mode

Chạy chương trình StudyModeProcess()

End Menu Mode

Hiển thị led báo chế độ Run Mode

64

Chờ và kiểm tra nút nhấn . Gán System Status ứng với nút nhấn vừa nhận


Chương trình con khở i tạo các module h ệ thống: InitSystem() InitSystem()

Start

Khởi tạo bộ ADC (ngắt mỗi

khi lấy mẫu giá trị mới)

Khởi tạo UART0 tốc độ Baud 9600 để giao tiếp module reader RFID ACE630 (Không cho phép ngắt)

Cho phép ngắt toàn cục

Khởi tạo bộ QEI (ngắt tràn timer mỗi 20ms)

Khởi tạo UART2 tốc độ Baud 115200 để giao tiếp với module

Cấu hình timer 1 ngắt mỗi 100ms dùng để quét nút nhấn

Khởi tạo bộ PWM

Lấy giá trị ADC cũ từ SoftEEPROM

Khởi tạo System Timer ngắt 1ms dùng tạo delay

Khởi tạo SoftEEPROM với vùng nhớ từ 0x30000 đến 0x31000

Cấu hình 2 led dùng làm báo mode

SystemStatus = MenuMode

Khởi tạo Timer 0 ngắt mỗi 5ms dùng chạy PID dò line

End

eZ430-RF2500

Chương trình con cho phép robot bắt đầ u hoạt động: RunModeProcess() RunModeProcess()

Start

Chờ bản tin RFID từ RFID reader

Gửi thông tin nhận từ RFID reader lên Master

thông qua mạng RF

Cho phép robot

trở về vị trí Home Navigation_Robot GoHome()

65

Chờ nhận bản tin từ Master , tách lấy địa chỉ cần đến của gói hàng và đường đi

Gửi yêu cầu đường trở về đến Master, chờ nhận bản tin từ Master và tách lấy đường đi về vị trí Home

Cho phép robot

chạy Navigation_Robot Run()

Chờ đến khi gói hàng được lấy ra khỏi robot


Chương trình con họ c line: StudyModeProcess() StudyModeProcess()

Hiển thị led báo chế độ Study

Start

Mode

Chờ kiểm tra nút nhấn

Nút nhấn mới nhận là Enter

Sai

SystemStatus = MenuMode

Đúng Lấy trung bình 5 mẫu từ bộ ADC của 6 cảm biến dùng xác định line (5 mẫu này tương ứng với line trắng)

Chờ nút nhấn

Lưu giá trị vào

Lấy trung bình 2 giá trị ADC tương đương với line trắng và line đen dùng làm ngưỡng

EEPROM

Lấy trung bình 5 mẫu từ bộ ADC của 6 cảm biến dùng xác định line (5 mẫu này tương ứng với line đen)

Lưu giá trị vào EEPROM

Nháy led hiển thị

End

Chờ nút nhấn

xét line

Chương trình con ngắt UART2 dùng đọ c các bản tin từ module eZ430-RF2500 truy ền về: Chương trình ngắt UART 2

Start

‘c’

Gửi trả lại 1 bản tin với byte chỉ mode là ‘a’ báo sự tồn tại của robot

Mỗi lần vào ngắt sẽ nhận một byte cho đến khi đủ 8

Đủ 8 byte

Kiểm tra byte chỉ mode của bản tin

‘s’

Toggle cờ báo dừng của robot

‘d’

Lọc lấy địa chi của gói hàng đang chở

byte

Chưa đủ

End

66


Chương trình ngắt UART0 để nhận các b ản tin từ module eZ430-RF2500: Ngắt UART 0

Start

Xóa cờ ngắt Lưu ký tự mới nhận được vào biến temp

Temp = 0x02?

YES

Không cho phép ngắt UART0 xảy ra

Chờ nhận đủ 10 ký tự RFID rồi thông báo cho hệ thống biết đã nhận được bản tin RFID, có thể bắt đầu tạo bản tin gửi lên master

NO

End

Chương trình con ngắt tràn timer module QEI dùng tính v ận tốc hiện tại của động cơ và tính PID: Ngắt tràn timer của module QEI

Cập nhật vị trí mới của động cơ (số xung theo hướng )

Xóa cờ báo ngắt

Start

tràn timer QEI

End

Truyền thông số mới sau khi tính PID cho bộ PWM 0 kênh tương ứng với động cơ

67

Khởi tạo lại giá trị vị trí ban đầu (250000)

Tính vận tốc hiện tại của động cơ và gọi hàm tính PID

vận tốc


Chương trình con: LinePIDControl()

Chương trình ngắt timer0 dùng tính PID khi dò line:

LinePIDControl ()

Ngắt Timer 0 Start

Start

Đọc giá trị line hiện tại và gán sai số so với trạng thái chạy line thẳng

Xóa cờ ngắt Đưa sai số lấy được ở trên vào hàm tính PID

Gọi hàm LinePIDControl()

Lấy kết quả của phép tính PID trên làm gia

số cộng vào vận tốc hiện tại của bánh trái và trừ đi đối với bánh phải

End

End

68


Chương trình con điề u khiển robot đi thẳng số line quy định: GoStraight(số line cần vượ t qua, vậ n tố c robot) GoStraight( Số line cần bắt , vận tốc robot )

Start

Cài đặt vận tốc cho robot và bật chế độ dò line sử dụng PID

Chưa bắt được

Kiểm tra số line bắt được đã đủ chưa

Chưa đủ

Đọc giá trị line mới

Kiểm tra đã bắt được line trắng bằng ít nhất 5 cảm biến chưa?

Đã bắt được

Đã đủ


Tăng số line bắt được lên 1

Chưa thoát

End

Đã thoát

Kiểm tra đã thoát được line trắng chưa?

69


Chương trình con điề u khiển robot quẹo trái: TurnLeft(v ậ n tố c quay) TurnLeft( vận tốc quay )

Start

Cài đặt vận tốc quay, Đọc giá trị line mới

bánh trái quay lùi,

bánh phải quay tiến


Chưa bắt được Kiểm tra đã bắt được line trắng bằng cảm biến tận cùng bên trái

Đã bắt được

Kiểm tra đã bắt được line trắng bằng 2 cảm biến chính giữa


không?

không?

Đã bắt được End

Chương trình con điề u khiển robot quẹo phải: TurnRight(vậ n tố c quay) TurnRight( vận tốc quay )

Start

Cài đặt vận tốc quay , bánh trái quay tiến, bánh phải quay lùi



Chưa bắt được Kiểm tra đã bắt được line trắng bằng cảm biến tận cùng bên phải không?

Đã bắt được


Kiểm tra đã bắt được line trắng bằng 2 cảm biến chính giữa không?

Đã bắt được End

70


Chương trình điều khiển robot bắt line ở góc trái (line này chỉ có thể bắt đượ c bằng 3 hoặc 4 cảm biến bên trái): GetLineCorner(vậ n tố c robot) GetLineCorner ( vận tốc robot )

Start

Cài đặt vận tốc cho robot và bật chế độ dò line sử dụng PID



Kiểm tra đã bắt được line trắng bằng ít nhất 2 cảm biến ngoài cùng bên trái chưa?

Đã bắt được Chưa thoát Kiểm tra đã thoát được line trắng chưa?


Đã thoát End



Giải thuật định vị trên robot slave:

−

Để định vị robot, mỗi robot slave sẽ có 3 bi ến toàn cục lưu giá trị đại diện cho

hướ ng di chuyển hi ện t ại CurrentDirection, vị trí point hiện t ại CurrentPoint, vị trí block hiện tại CurrentBlock (vị trí point và block hi ện tại là vị trí ở lần cậ p nhật gần nhất). −

Để định v ị hướng robot slave đang di chuyể n, ta sẽ c ậ p nh ật bi ến CurrentDirection

mỗi khi robot quay đầ u với quy định hướ ng cố định như sau: Direction + 1

Direction - 1 1 = n o i t c e r i D

Direction = 0

2 = n o i t c e r i D

D i r e c t i o n = 3

Hình 4.27 Quy định về biến định vị hướ ng của robot

71


Lưu đồ gi ải thuật chương trình con UpdateDirection(hướng quay) dùng để cậ p nh ật hướ ng mỗi khi robot thực hiện một chương trình quay đầ u.

UpdateDirection ( hướng quay )

Start

Hướng quay?

Quẹo trái

CurrentDirection = 4?

CurrentDirection + 1

YES

CurrentDirection = 0

NO

Quẹo phải

CurrentDirection = 0?

NO

CurrentDirection - 1

End

YES

CurrentDirection = 4

−

Để định vị vị trí point, robot slave sẽ chứa bản đồ sàn làm vi ệc dướ i dạng một mảng

hai chiều MapLocation[số lượ ng point trong 1 block][4]: o

Chiều thứ nhất chỉ vị trí point robot cậ p nhật gần nhất .

o

Chiều thứ hai chỉ vị trí point tương ứng vớ i hướ ng hiện tại của robot.

Ví dụ nếu vị trí point lần cậ p nhật cuối cùng của robot là 5 và hướng robot đang chạ y là

hướ ng qua trái (số 2) thì khi robot gặ p line m ớ i và gọi hàm cậ p nhật vị trí point mớ i thì k ết quả tr ả về sẽ là MapLocation[5][2]. −

Để định v ị v ị trí block, biến CurrentBlock sẽ đượ c cộng thêm hoặc tr ừ b ớ t m ỗi khi

robot cậ p nhật vị trí point mớ i là vị trí point bắt đầu của 1 block m ớ i. 

Giải thuật tìm đườ ng (navigation) trên robot slave:

Theo quy đị nh về cách xây d ựng sàn làm vi ệc thì để đi từ một kho đến một kho khác, robot phải có 2 lần quẹo trái hoặc phải. Theo đó ta chỉ cần định vị đượ c robot và d ẫn đường robot đi thẳng đến v ị trí point đượ c master cho phép qu ẹo và ra lệnh cho robot quẹo 1 lần, tiế p t ục đi 72

THIẾT K Ế VÀ THI CÔNG thẳng đến v ị trí point đượ c master cho phép qu ẹo và ra l ệnh cho robot qu ẹo l ần th ứ hai, tiế p tục đi thẳng đến khi đến đượ c kho. Nghiên cứu ảnh hưở ng giữa các yếu tố: 

So sánh gi ữa vị trí block hiện tại và vị trí block cần đến.



Vị trí point cần đến.



Vị trí point hiện tại.

Từ các yếu t ố trên ta có th ể rút ra đượ c k ết luận trong trườ ng h ợ p nào robot c ần qu ẹo trái

trướ c, quẹo ph ải sau hay qu ẹo phải trướ c, quẹo phải sau hay c ả hai lần quẹo đều trái hay cả hai lần quẹo đều phải. Ta có giải thuật dẫn đường cho robot như sau: Navigation_ RobotRun()

Start

Block hiện tại trùng block cần đến

NO

Point hiện tại là 0

NO

Point cần đến là 0

NO

Giá trị block hiện tại nhỏ hơn giá trị block cần đến

NO

Quẹo trái tại block hiện tại và point

Quẹo trái tại block của kho hàng và

do master cung cấp (11)

point do master cung cấp (2)

Quẹo phải tại block hiện tại và

YES


YES


NO

Quẹo trái tại block hiện tại và point do master cung cấp (11)

Quẹo phải tại block của kho hàng và point do

master cung cấp (4)



YES

YES

Point cần đến YES

là 0

NO


NO










YES


YES


YES

Đi thẳng đến point của kho hàng

End

73

NO

Quẹo phải tại block hiện tại và point do master cung cấp (7)










Các chương trình con dùng quẹ o trái, quẹo phải tại vị trí point quy định đượ c sử dụng trong chương trình dẫn đườ ng trên: Chương trình con điề u khiển robot đi thẳng, cậ p nhật vị trí cho đến khi gặ p vị trí point đượ c chỉ định thì quẹo trái: Navigation_TurnLeftCondition(vị trí block, vị trí point quẹ o trái) Navigation_TurnLe ftCondition (Block, Point)

Start

Đi thẳng 1 line Không trùng

Cập nhật vị trí mới Gửi vị trí về cho master

Đi thẳng 1 line và kiểm tra cờ báo dừng

Có

Cờ báo dừng?

Không có

Kiểm tra block và point hiện tại có trùng với block và point cần đến không?

trùng

Quẹo trái

End

74


Chương trình con điề u khiển robot đi thẳng, cậ p nhật vị trí cho đến khi gặ p vị trí point đượ c chỉ định thì quẹo phải: Navigation_TurnRightCondition(vị trí block, vị trí point quẹ o phải) Navigation_TurnRight Condition (Block, Point)

Start




Có

Cờ báo dừng?

Không có

Kiểm tra block và point hiện tại có trùng với block và point cần đến không?

trùng

Quẹo phải

End

75


Chương trình con điề u khiển robot vừa đi thẳng vừa cậ p nhật vị trí và dừng lại tại vị trí point đượ c chỉ định: Navigation_GoStraightCondition(vị trí point) Navigation_GoStraigh tCondition (Point)

Start




Có

Cờ báo dừng?

Không có

Kiểm tra point hiện tại có trùng với point cần đến không?

trùng

Dừng robot

End

4.5 Chương trình trên máy tính: Trên máy tính gồm hai chương trình: 

Chương trình gửi thông số về vị trí kho hàng ứng v ớ i từng mã số RFID viết bằng

ngôn ngữ C#.

76

THIẾT K Ế VÀ THI CÔNG 

Chương trình hiển thị (vẽ lại) vị trí của các xe đã đăng ký trên sàn hoạt độ ng viết

bằng Python 2.7.3. Một lưu ý là hai chương trình trên không thể ch ạy song song cùng nhau được. Đây là mộ t hạn chế và sẽ đượ c sửa ở phiên bản sau. 4.5.1 Chương trình viế t bằ ng ngôn ng ữ C#:

Chương trình có giao diện như sau:

Hình 4.28 Giao di ện chương trình viế t trên C#

Bên trái là các mã s ố của của các th ẻ Tag và kho hàng quy đị nh của thẻ tag. Bên ph ải là sơ

đồ sàn hoạt động và các kho hàng. Để sử dụng, ngườ i vận hành phải tiến hành k ết nối máy tính vớ i cổng COM như sau:

Hình 4.29 C ử a sổ thiế t lậ p k ết nố i UART

77


Để thay đổi vị trí kho, bấm vào nút kho và l ựa chọn kho mong muốn của gói hàng.

Hình 4.30 C ử a sổ thay đổ i vị trí kho hàng

Sau đó, ứng vớ i vị trí mã số RFID được để sẵn, ngườ i vận hành có thể nhậ p vị trí kho hàng mớ i và gửi xuống vi điều khiển master. 4.5.2 Chương trình viế t bằ ng ngôn ng ữ Python 2.7.3:

Chương trình có tác dụng hiển thị vị trí các robot slave trong m ạng ở vị trí lần cậ p nhật cuối cùng.

Hình 4.31 C ử a sổ chương trình trên Python

78

VẬ N HÀNH VÀ K ẾT QUẢ

CHƯƠNG 5

VẬN HÀNH VÀ KẾT QUẢ

5.1 Vận hành: Cấ p nguồn cho master, k ết nối vớ i máy tính.

Khởi động chương trình trên C# để cài đặt vị trí kho cho các th ẻ Tag.

Tắt chương trình C#, khởi động chương trình trên Python.

79


Cấ p nguồn cho robot slave (k ết nối nguồn từ pin Lipo, bật công t ắc).

Bấm nút nhấn S1 1 l ần để ch ọn ch ế độ h ọc line cho robot. Lúc này 2 led báo tr ạng thái sẽ sáng thông báo robot ở chế độ học line và bấm nút S2 m ột lần để vào chế độ học line. Đặt robot ở phần nền đen, bấ m nút nhấn S2 1 lần để học nền đen. Đặt robot ở phần line tr ắng bấm

80

VẬ N HÀNH VÀ K ẾT QUẢ nút S2 1 l ần để học line tr ắng. Hai led báo tr ạng thái sẽ nháy 1 l ần để thông báo đã họ c line xong. Bấm nút nhấn S1 1 lần để chọn chế độ chạy. Đặt robot ở vị trí tổng kho (block 2, point 13). Bấm nút nhấn S2 1 l ần để vào ch ế độ chạy. Robot sẽ chờ tín hiệu từ module reader RFID. Quẹt thẻ Tag qua bộ thu RFID và robot sẽ bắt đầu chạy chương trình chính. Sau khi robot chạy tr ở về vị trí tổng kho thì nó s ẽ lại chờ tín hiệu thẻ Tag RFID m ớ i.

81


5.2 Kết quả: 

Về phần cứng:

Mô hình sàn làm việc của robot thiết k ế với kích thướ c phù hợp để robot di chuyển và

tránh, nhường đườ ng nhau. Vi ệc thi công sàn làm vi ệc t ốt cho phép robot xác định đúng line và di chuyển tốt. Mô hình robot slave thi ết k ế nhỏ gọn, động cơ sử dụng trên robot đáp ứng đượ c yêu cầu di chuyển của đề tài. 

K ết quả thực hiện chương trình:

Robot slave: 

Chạy bám theo line khá t ốt, cậ p nhật vị trí liên tục về master đạt yêu cầu đề ra.



Nhận diện gói hàng và v ận chuyển về đúng kho được quy đị nh ở master.



Có khả năng dừng lại tại những giao lộ khi xảy ra xung độ t tuyến đườ ng.



Có khả năng xế p hàng ở vị trí kho chính.



Truyền vị trí liên tục về master.

Master: 

Giao tiế p chính xác v ớ i robot slave v ề mặt địa chỉ cũng như về dữ liệu.



Cậ p nhật và vẽ lại vị trí của các robot slave chính xác.

Tóm l ại:

K ết qu ả th ực hi ện luận văn đã đáp ứng đượ c các yêu c ầu mà luận văn hướ ng tới đó là xây dựng thành công m ột h ệ th ống robot chạy vận chuyển hàng hóa đến nơi mong muố n sử d ụng mạng không dây.

82

K ẾT LUẬ N

KẾT LUẬN

CHƯƠNG 6

6.1 Nhữ ng kết quả đạt đượ c: 

Thành công trong vi ệc sử dụng và k ết hợ p nhiều module trên vi điều khiển ARM Cortex_M3 của hãng Texas Instrument.



Sử dụng thành công module eZ430- RF2500 để giao tiế p không dây thông qua mạng RF và cho k ết quả khá t ốt.



Xây dựng thành công mô hình robot dò line.



Xây dựng thành công giao th ức giao tiế p giữa một master và nhi ều slave.



Thành công trong vi ệc tìm hiểu và lậ p trình trên Python và C#.

6.2 Khả năng ứ ng dụng: 

Xét về robot slave: o

Có thể ứng dụng khả năng di chuyển theo line đị nh sẵn của robot slave trong các ứng dụng cần robot di chuyển giữa 2 điểm cố định ứng dụng nhiều trong các kho hàng nh ỏ hoặc một số sân chơi robot cho sinh viên.



Xét về giao thức giao tiế p giữa một master và nhi ều slave: o

Ứ ng dụng đượ c giao thức này trong một số ứng dụng mạng vừa và nh ỏ trong điều khiển khi các slave ch ỉ truyền comment kèm theo d ữ li ệu không lớ n về master.



Toàn bộ lu ận văn này có thể ứng dụng vào thực tiễn là một h ệ th ống robot tự động vận chuyển các gói hàng đượ c đánh dấu về nơi theo quy định sẵn. Khi lậ p trình, tôi

đã cố g ắng để chương trình có thể ứng dụng vào một hệ thống mạng lớ n mà không phải thay đổi nhiều.

6.3 Nhữ ng hạn chế còn tồn tại và hướ ng giải quyết: 

Mô hình robot slave nh ỏ, không đáp ứng đượ c yêu cầu vận chuyển các s ản phẩm lớ n. 

Khi thay đổi mô hình slave m ớ i l ớn hơn để có khả năng vận chuyển nh ững gói hàng lớ n thì chỉ c ần thay đổi ph ần công su ất cung cấp cho động cơ, các vấn đề về giải thuật vẫn tương tự như mô hình nhỏ hoặc nếu có ph ải thay

đổi thì cũng tương đố i dễ dàng. 83

K ẾT LUẬ N 

Mô hình sàn làm vi ệc còn khá ph ức tạ p khi mỗi line định vị cần có 1 line dừng. 

Vấn đề có thể đượ c gi ải quyết khi thay s ự ki ện phát hiện line dừng b ằng s ự kiện đi đượ c m ột quãng đườ ng nhất định so với line định v ị trước đó. Điều này có thể thực hiện được do động cơ trên robot có trang bị encoder cho

phép đo được quãng đườ ng di chuyển (không chính xác tuy ệt đối). 

Các robot trong luận văn không có khả năng nhận diện vật cản trướ c mặt (ví dụ như một robot khác trong hệ th ống), điều này khi ến cho vi ệc xế p hàng chờ nhận hàng ở kho hàng t ổng khó khăn. 

Có thể s ử d ụng thêm một c ảm bi ến đo khoảng cách (như cả m bi ến siêu âm, sonar…) gắn ở đầu robot cho phép robot nh ận diện vật cản trướ c mặt và duy trì khoảng cách an toàn v ớ i vật cản đó.



Hệ thống đượ c thi công trong lu ận văn này đòi hỏ i mỗi gói hàng phải đượ c gắn 1 thẻ Tag RFID, như vậy đòi hỏ i phải có số lượ ng lớ n thẻ RFID và cần có công đoạ n phân loại gói hàng nào g ắn v ớ i th ẻ Tag nào khá b ất ti ện đối v ớ i m ột ứng dụng c ần vận chuyển các gói hàng nh ậ p về từ nhiều nguồn khác nhau. 

Hệ thống có thể đượ c thay đổi sử dụng công ngh ệ đọc mã vạch thay cho công ngh ệ RFID để tận dụng thông tin có s ẵn trên bao bì sản phẩm.



Kiểm tra khả năng giao tiế p thông qua m ạng RF cho th ấy đôi khi còn bị m ất frame truyền buộc sender ph ải gửi lại bản tin một lần n ữa. N ếu h ệ thống có ít slave điều này ch ấ p nhận được nhưng vớ i hệ thống lớ n thì việc truyền bản tin, đợ i thờ i gian time out r ồi truyền lại như vậy bu ộc master phải xử lý nhiều hơn và có thể gây ra sai khi thờ i gian tr ễ quá l ớ n. 

Hệ thống có thể chuyển sang sử dụng mạng Ethernet không dây giao th ức

TCP/IP để đảm bảo tốc độ, sự ổn định trên đườ ng truyền cũng như khả năng mở r ộng hệ thống. 

Chương trình trên máy tính không hoàn toàn dễ s ử d ụng do có đến 2 chương trình cho 2 mục đích khác nhau, dễ gây r ối cho ngườ i s ử d ụng và ngườ i s ử d ụng không thể thay đổi vị trí kho hàng trong khi vẫn đang quan sát sự di chuyển của các robot. 

Do thờ i gian thực hi ện luận văn khá ngắn nên tôi không có đủ th ờ i gian tìm hiểu thật k ỹ một chương trình và hoàn thành tấ t cả như mong muố n trên

chương trình đó.

84

K ẾT LUẬ N 

Chương trình chưa có khả năng chạy real time. 

Có thể sử dụng khả năng đo quãng đườ ng bằng encoder của robot để cậ p nhật vị trí robot liên t ục hơn và các line đị nh vị để định vị lại vị trí chính xác của robot.

6.4 Hướ ng phát triển của đề tài: Đề tài có thể phát triển các hướ ng sau: 

Phát triển về sàn hoạt động: xây dựng sàn hoạt động r ộng lớn hơn vớ i nhiều đườ ng

line chính cho phép các robot có nhi ều lựa chọn khi di chuyển giữa các block, h ạn chế tối đa sự xung đột về vị trí, cho phép v ận chuyển hàng nhanh hơn. 

Phát triển về robot slave: xây d ựng robot slave l ớn hơn cho phép mang theo hàng

hóa nhiều hơn, tiết kiệm thờ i gian và chi phí trong v ận chuyển. 

Phát triển về phương pháp nhận diện gói hàng: ngoài vi ệc sử dụng công ngh ệ RFID

để nhận diện gói hàng có th ể phát triển thêm một phương pháp khác là sử dụng công nghệ đọc mã vạch. Tùy vào t ừng ứng dụng cụ thể mà có thể lựa chọn giữa hai phương pháp này. 

Phát triển về phần m ềm: xây dựng một ph ần mềm qu ản lý chuyên nghi ệp hơn, cho

phép vừa cậ p nhật vị trí robot liên tục, vừa thay đổi kho hàng ứng vớ i các gói hàng khác nhau cùng m ột lúc, tạo sự tiện lợi cho ngườ i vận hành.

85

TÀI LIỆU THAM KHẢO

TÀI LIỆU THAM KHẢO: 

Tài liệu trong nướ c:

[1] Nguyễn Th ị Phương Hà và Huỳnh Thái Hoàng (2002). Lý thuy ết điề u khiể n t ự động. Nhà xuất bản đại học quốc gia TPHCM. [2] Nguyễn Đức Thành (2005). Đo lường và điề u khiể n b ằ ng máy tính . Nhà xu ất b ản đại học quốc gia TPHCM. [3] Nguyễn Vũ Quang, Đoàn Nam Thái (2005). Robot tự hành. Đồ án môn h ọc. Đại học Bách khoa – Đại học quốc gia TPHCM. 

Tài liệu nướ c ngoài:

[4] Carole Wilson (2007). Exploring Information and Software Technology 4 th Ed. Cambridge University Press. [5] John A. Shaw, (2003) The PID Control Algorithm - How it works, how to tune it, and how to use it , 2nd edition, (ebook).

[6] Ming-Hsin Chen, Yu-Te Fu, Kuang-Shun Ou, and Kuo-Shen Chen, (2012), Development of Navigation Schemes for Grouped Mobile Robots Leading to Biomimetic Applications. National Cheng-Kung University, Taiwan.

[7] Thomas Bräunl (2006), EMBEDDED ROBOTICS, School of Electrical, Electronic and Computer Engineering The University of Western Australia, 2 nd edition. 

Tài liệu từ các website: http://www.cnx.org http://e2e.ti.com/ http://en.wikipedia.org/wiki/ http://www.payitforward.edu.vn/forum/forum/ http://www.kivasystems.com/ http://www.societyofrobots.com/ http://letsmakerobots.com/

86

PHỤ LỤC

PHỤ LỤC Một số đoạn code sử dụng trong chương trình 

Các chương trình con điề u khiển robot di chuyển theo line:

/* ---- Chương trình con kiể m tra giá tr ị line hiện t ại và tr ả về sai số line-------------------*/ int32_t LineValue(uint32_t mavach) { static int32_t Line_Sensor_old; static int32_t erro_previous; uint32_t Line_Sensor_flag = 0; if(mavach==0) { Line_Sensor_flag = 1; } else { Line_Sensor_flag = 0; } if(Line_Sensor_flag==0) { Line_Sensor_old = mavach; //neu hien dang nam line trang } switch(mavach) { case 0x00: if(Line_Sensor_old&0x01) { erro_previous=18; return 18; } else { erro_previous=-18; return -18; } case 0x0C: erro_previous=0; return 0; case 0x04: erro_previous=2; return 2; case 0x06: erro_previous=4; return 4; case 0x02: erro_previous=6; return 6; case 0x03: erro_previous=12; return 12; case 0x01: erro_previous=14; return 14;

87

PHỤ LỤC case 0x08: erro_previous=-2; return -2; case 0x18: erro_previous=-4; return -4; case 0x10: erro_previous=-6; return -6; case 0x30: erro_previous=-12; return -12; case 0x20: erro_previous=-14; return -14; default: return (erro_previous); (erro_previous); } }

/* ---- Chương trình con tính PID dò line cho ra vậ n t ố ốc cần thêm/b ớ t vào 2 bánh-----------*/ void LinePIDControl(void) LinePIDControl(void) { const int32_t max_motor_res = 3000;//max value that is added to velocity volatile int32_t motor_res=0; static volatile int32_t I=0; static volatile int32_t error_1; static volatile int32_t error_2; static volatile int32_t error; volatile uint32_t Line_Sensor; static volatile int32_t LinePIDCounter = 0; LinePIDCounter++; if(LinePIDCounter == 100) { LinePIDCounter = 0; I = 0; } Line_Sensor = ADCValue_to_Bin(); ADCValue_to_Bin(); error_2 = error_1; error_1 = error; error = LineValue(Line_Sensor); LineValue(Line_Sensor); I += error; //limit I value if (I>=10) I = 10; else if (I<=-10) I = -10; motor_res = 210*error +5*I+ 0*(error-error_1); 0*(error-error_1); // limit value added/subtracted added/subtracted to motor speed if (motor_res > (max_motor_res)) (max_motor_res)) { motor_res = max_motor_res; } else if (motor_res < (-max_motor_res)) (-max_motor_res)) {

88

PHỤ LỤC motor_res = (-max_motor_res); (-max_motor_res); } ROM_IntMasterDisable(); SetVelocityppsQEI0 = SetVelocityRobot SetVelocityRobot - motor_res; motor_res; SetVelocityppsQEI1 = SetVelocityRobot SetVelocityRobot + motor_res; motor_res; ROM_IntMasterEnable();

// banh banh phai // banh banh trai

} 

Các chương trình con điề u khiển robot đị nh vị.

/* ---- Khai báo m ảng hai chi ều đóng vai trò là bản đồ trên slave----------------------------*/ const uint32_t maplocation[Number_location][4] maplocation[Number_location][4] = { {Unknown_location,10,Unknown_locati {Unknown_location,10,Unknown_location,7}, on,7}, //0 {Unknown_location,Unknown_location {Unknown_location,Unknown_location,3,Unknown_locati ,3,Unknown_location}, on}, {Unknown_location,Unknown_location,1,8}, {Unknown_location,10,2,Unknown_location}, //3 {5,Unknown_location,Unknown_location,9}, {6,11,Unknown_location,Unknown_location}, //5 {4,Unknown_location,Unknown_locatio {4,Unknown_location ,Unknown_location,Unknown_locatio n,Unknown_location}, n}, {Unknown_location,Unknown_location,1,8}, {5,Unknown_location,Unknown_location,9}, //8 {13,Unknown_location,Unknown_locati {13,Unknown_locatio n,Unknown_location,Unknown_locati on,Unknown_location}, on}, {Unknown_location,Unknown_location,0,Unknown_location}, {Unknown_location,10,2,Unknown_location}, {6,11,Unknown_location,Unknown_location}, {Unknown_location,12,Unknown_locati {Unknown_location,1 2,Unknown_location,Unknown_locati on,Unknown_location} on} };

//10

/* ---- Chương trình con gử i vị trí hiện t ại của slave lên master---------------------------------*/ void Location_SendMaster(void) Location_SendMaster(void) { // Send current location location to master // frame: 'p'|direction|block|point 'p'|direction|block|point|0 |0 volatile uint8_t tempdata[5]; tempdata[0] = 'p'; tempdata[1] = CurrentDirection; tempdata[2] = CurrentLocationBlock; CurrentLocationBlock; tempdata[3] = CurrentLocationPoint; CurrentLocationPoint; tempdata[4] = 0; RF2500Send(MasterAddress, RF2500Send(MasterAddress, tempdata); }

/* ---- Chương trình con cậ p nhật vị trí hiện t ại của slave-----------------------------------------*/ void Location_UpdatePosition(void) Location_UpdatePosition(void) { volatile uint32_t temp; temp = maplocation[CurrentLocationPoint] maplocation[CurrentLocationPoint][CurrentDirectio [CurrentDirection]; n]; if(temp == Unknown_location) Unknown_location) while(1);

//error

// update location block if((CurrentLocationPoint if((CurrentLocation Point == 1) && (temp == 3)) CurrentLocationBlock--; if((CurrentLocationPoint if((CurrentLocation Point == 6) && (temp == 4))

89

PHỤ LỤC CurrentLocationBlock++; CurrentLocationPoint CurrentLocationPoint = temp; // Send the current current position to master master Location_SendMaster(); }

/* ---- Chương trình con cậ p nhật hướng sau khi quay đầ u của slave--------------------------*/ void Location_UpdateDirection(uint32 Location_UpdateDirection(uint32_t _t TurnLeftRight) TurnLeftRight) { if(TurnLeftRight == TurnLeft_location) TurnLeft_location) { CurrentDirection++; if(CurrentDirection == 4) CurrentDirection = 0; } else if(TurnLeftRight == TurnRight_location) TurnRight_location) { if(CurrentDirection if(CurrentDirection == 0) CurrentDirection = 4; CurrentDirection--; } } 

Các chương trình con có tác dụ ng dẫn đườ ng ng robot slave:

/* ---- Chương trình con điều khiển robot đi thẳng đến khi bắt đượ c 1 line----------------------*/

void Navigation_GoStraight(void) Navigation_GoStraight(void) { // run until get line line and out of line line (not stop) GoStraight(1, Navigation_normalSpeed, Navigation_normalSpeed, 0); }

xung độ t, /* ---- Chương trình con điề u khiển robot đi thằng đến khi b ắt đượ c 1 line và ki ểm tra cờ xung xung đột thì dừ ng xung đột b ị xóa----------------------*/ nếu có cờ xung ng lại đến khi cờ xung xóa----------------------*/ void Navigation_GoStop(void) Navigation_GoStop(void) { // run until get line line and check the collision traffic traffic flag volatile uint32_t Line_Sensor; const uint32_t MASK0 = 0x3e; const uint32_t MASK1 = 0x1f; volatile uint32_t temp0, temp1; SetRobotSpeed(Navigation_normalSpeed); while(1) { Line_Sensor = ADCValue_to_Bin(); ADCValue_to_Bin(); temp0 = Line_Sensor & MASK0; temp1 = Line_Sensor & MASK1; if(temp0 == MASK0 || temp1 == MASK1) { WaitingForNewADCValue(); WaitingForNewADCValue();//read WaitingForNewADCVal ue();//read line again Line_Sensor = ADCValue_to_Bin(); ADCValue_to_Bin(); if(temp0 == MASK0 || temp1 == MASK1) { break;

90

PHỤ LỤC } } } // Now Robot has reach the line to stop. Check the collision traffic flag // to decide whether to stop or run continuously if(CollisionTrafficFlag) { // run slower and when going out of line, stop robot SetRobotSpeed(Navigation_slowSpeed); } // waiting for going of out line while(1) { Line_Sensor = ADCValue_to_Bin(); temp0 = Line_Sensor & 0x21; if((temp0 == 0)) { break; } } LedDisplay_Toggle(); if(CollisionTrafficFlag) { // stop robot StopRobot(); } while(CollisionTrafficFlag); } /* ---- Chương trình con điều khiển robot đi thẳng đến khi bắt đượ c line ở góc và quẹo trái----*/

void Navigation_TurnLeft_corner(void) { // Run, get line corner, turn left GetLineCorner_left(Navigation_normalSpeed); TurnLeft(Navigation_turnSpeed); // update current direction Location_UpdateDirection(TurnLeft_location); } /* ---- Chương trình con điều khiển robot đi thằng đến khi bắt đượ c 1 line và qu ẹo trái----*/

void Navigation_turnLeft(void) { // Run, get line, turn left GoStraight(1, Navigation_normalSpeed, 1); TurnLeft(Navigation_turnSpeed); // update current direction Location_UpdateDirection(TurnLeft_location); } /* ---- Chương trình con điều khiển robot đi thằng đến khi bắt đượ c 1 line và qu ẹo phải----*/

void Navigation_turnRight(void) { // Run, get line, turn right

91

PHỤ LỤC GoStraight(1, Navigation_normalSpeed, 1); TurnRight(Navigation_turnSpeed); // update current direction Location_UpdateDirection(TurnRight_location); } /* -- Chương trình con kiểm tra v ị trí của robot có đang ở góc và cần phải quẹo trái không--*/

void Navigation_CheckPosition(void) { // Check whether the robot must turn left at the next line (corner) or not if((CurrentLocationPoint == 0) || (CurrentLocationPoint == 9) || (CurrentLocationPoint == 10) || (CurrentLocationPoint == 13)) Navigation_TurnLeft_corner(); else if((CurrentLocationPoint != 6) && (CurrentLocationPoint != 1)) { Navigation_GoStraight(); } } /* -- Chương trình con điều khiển robot đi thằng đến khi gặp đượ c v ị trí quy đị nh thì quẹo trái--*/

void Navigation_TurnLeftCondition(uint32_t MidDesBlock, uint32_t MidDesPoint) { // This function will let the robot go straight and update position until // meet the condition in parameter. Then robot will turn left. while(1) { // run 1 line Navigation_GoStraight(); // update position Location_UpdatePosition(); // run stop line Navigation_GoStop(); if((CurrentLocationPoint == MidDesPoint) && (CurrentLocationBlock == MidDesBlock)) { break; } else { Navigation_CheckPosition(); } } // Now robot has at the right line and ready to turn left Navigation_turnLeft(); } /* -- Chương trình con điều khiển robot đi thằng đến khi gặp đượ c v ị trí quy đị nh thì quẹo phải--*/

void Navigation_TurnRightCondition(uint32_t MidDesBlock, uint32_t MidDesPoint) { // This function will let the robot go straight and update position until // meet the condition in parameter. Then robot will turn right. while(1) { // run 1 line Navigation_GoStraight();

92

PHỤ LỤC // update position Location_UpdatePosition(); // run stop line Navigation_GoStop(); if((CurrentLocationPoint == MidDesPoint) && (CurrentLocationBlock == MidDesBlock)) { break; } Navigation_CheckPosition(); } // Now robot has at the right line and ready to turn right Navigation_turnRight(); } /* -- Chương trình con điều khiển robot đi thằng đến khi gặp đượ c v ị trí quy đị nh thì dừ ng--*/

void Navigation_GoStraightCondition(uint32_t DesPoint) { // This function will let the robot go straight and update position until meet the // condition in parameter. Then robot will stop and wait for pakage picking // Robot will search for Destination point in the same block with current point while(1) { // run 1 line Navigation_GoStraight(); // update position Location_UpdatePosition(); // run stop line Navigation_GoStop(); if((CurrentLocationPoint == DesPoint)) { break; } Navigation_CheckPosition(); } StopRobot(); // don't use stop flag when go straight because of wanting robot stop // immediately waiting for package picking/laying } /* -- Chương trình con điều khiển robot đén vị trí được đị nh sẵn trong biến toàn cục--*/

void Navigation_RobotRun(void) { Navigation_CheckPosition(); if(CurrentLocationBlock == DestinationLocationBlock) { // Go straight until meet the right line (destination location point) Navigation_GoStraightCondition(DestinationLocationPoint); } else // go and search destination at another block { // Process and choose the right path if(CurrentLocationPoint == 13) { if(DestinationLocationPoint == 13) { if(CurrentLocationBlock > DestinationLocationBlock)

93

PHỤ LỤC { // The path corresponding to this condition Navigation_TurnLeftCondition(CurrentLocationBlock,11); Navigation_TurnLeftCondition(DestinationLocationBlock,2); } else// CurrentLocationBlock < DestinationLocationBlock { Navigation_TurnRightCondition(CurrentLocationBlock,12); Navigation_TurnRightCondition(DestinationLocationBlock,4); } Navigation_GoStraightCondition(13); // Robot stop at the stop line of (DestinationLocationBlock, 13) } if(DestinationLocationPoint == 0) { if(CurrentLocationBlock > DestinationLocationBlock) { // The path corresponding to this condition Navigation_TurnLeftCondition(CurrentLocationBlock,11); Navigation_TurnRightCondition(DestinationLocationBlock,3); } else // CurrentLocationBlock < DestinationLocationBlock { Navigation_TurnRightCondition(CurrentLocationBlock,12); Navigation_TurnLeftCondition(DestinationLocationBlock,5); } Navigation_GoStraightCondition(0); // Robot stop at the stop line of (DestinationLocationBlock, 0) } } else // CurrentLocationPoint = 0 { if(DestinationLocationPoint == 13) { if(CurrentLocationBlock > DestinationLocationBlock) { // The path corresponding to this condition Navigation_TurnRightCondition(CurrentLocationBlock,7); Navigation_TurnLeftCondition(DestinationLocationBlock,2); } else // CurrentLocationBlock < DestinationLocationBlock { Navigation_TurnLeftCondition(CurrentLocationBlock,8); Navigation_TurnRightCondition(DestinationLocationBlock,4); } Navigation_GoStraightCondition(13); // Robot stop at the stop line of (DestinationLocationBlock, 13) } if(DestinationLocationPoint == 0) { if(CurrentLocationBlock > DestinationLocationBlock) { // The path corresponding to this condition Navigation_TurnRightCondition(CurrentLocationBlock,7); Navigation_TurnRightCondition(DestinationLocationBlock,3); }

94

VMHoang_Robot Tu Dong

Recommend Documents