TRƯỜNG ĐẠI HỌC KỸ THUẬT – CÔNG NGHỆ CẦN THƠ
KHOA KỸ THUẬT CƠ KHÍ
---------- ----------
LUẬN VĂN TỐT NGHIỆP ĐẠI HỌC
THIẾT KẾ VÀ CHẾ TẠO MÔ HÌNH ROBOT SCARA
PHÂN LOẠI SẢN PHẨM THEO MÀU SẮC VÀ HÌNH DẠNG
CÁN BỘ HƯỚNG DẪN: SINH VIÊN THỰC HIỆN:
Ths: ĐƯỜNG KHÁNH SƠN PHAN PHÚC HẬU
MSSV: 1600061
Ngành: Công nghệ kỹ thuật
Cơ điện tử - 2016
Cần Thơ - 2020
Nhận xét của giáo viên hướng dẫn
...............................................................................
134 trang |
Chia sẻ: huong20 | Ngày: 13/01/2022 | Lượt xem: 472 | Lượt tải: 0
Tóm tắt tài liệu Luận văn Thiết kế và chế tạo mô hình robot scara phân loại sản phẩm theo màu sắc và hình dạng, để xem tài liệu hoàn chỉnh bạn click vào nút DOWNLOAD ở trên
.........................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
Cần Thơ, ngày tháng năm 2020
GVHD
Nhận xét của giáo viên phản biện
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
Cần Thơ, ngày tháng năm 2020
GVPB
CÔNG TRÌNH ĐƯỢC HOÀN THÀNH TẠI
TRƯỜNG ĐẠI HỌC KỸ THUẬT – CÔNG NGHỆ CẦN THƠ
Cán bộ hướng dẫn: Ths. ĐƯỜNG KHÁNH SƠN
Luận văn đại học được bảo vệ tại Trường Đại học Kỹ thuật – Công nghệ Cần Thơ
ngày 22 tháng 07 năm 2020.
Thành phần Ban chấm đánh giá luận văn đại học gồm:
1. Trưởng ban: ..........................................................................................................
..................................................................................................................................
2. Uỷ viên phản biện: ...............................................................................................
..................................................................................................................................
3. Thư ký ban chấm: .................................................................................................
..................................................................................................................................
Xác nhận của Ban chấm đánh giá sau khi luận văn đã được sửa chữa (nếu có).
TRƯỞNG BAN UỶ VIÊN PHẢN BIỆN THƯ KÝ
TRƯỜNG ĐẠI HỌC CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM
KỸ THUẬT - CÔNG NGHỆ CẦN THƠ Độc lập - Tự do - Hạnh phúc
KHOA KỸ THUẬT CƠ KHÍ
NHIỆM VỤ LUẬN VĂN ĐẠI HỌC
Họ và tên sinh viên: PHAN PHÚC HẬU MSSV: 1600061
Ngày, tháng, năm sinh: 30/07/1998 Nơi sinh: Cần Thơ
Ngành: Công nghệ kỹ thuật cơ điện tử Mã số: 7510203
TÊN ĐỀ TÀI:
THIẾT KẾ VÀ CHẾ TẠO MÔ HÌNH ROBOT SCARA PHÂN LOẠI SẢN
PHẨM THEO MÀU SẮC VÀ HÌNH DẠNG
NHIỆM VỤ VÀ NỘI DUNG
Nhiệm vụ:
Ứng dụng công nghệ xử lý ảnh cho Robot SCARA để phân loại sản phẩm theo
màu sắc và hình dạng từ đó đưa ra quyết định cho cánh tay.
Nội dung:
- Thiết kế, chế tạo mô hình Robot Scara hoàn chỉnh.
- Thiết kế mạch điều khiển cho Robot Scara.
- Xây dựng giao diện người dùng trên máy tính bằng phần mềm Matlab.
- Ứng dụng xử lý ảnh vào mô hình Robot để phân loại sản phẩm.
THỜI GIAN GIAO ĐỀ TÀI: 10/01/2020
THỜI GIAN HOÀN THÀNH: 07/07/2020
Cần Thơ, ngày tháng năm 2020
KHOA KỸ THUẬT CƠ KHÍ
LỜI CẢM ƠN
LỜI CẢM ƠN
Để hoàn thành đề tài luận văn và kết thúc khóa học, em xin bày tỏ lòng biết ơn
sâu sắc đến trường Đại học kỹ thuật – công nghệ Cần Thơ đã tạo điều kiện cho em
có môi trường học tập tốt trong suốt thời gian học tập và nghiên cứu tại trường. Đặc
biệt, em xin chân thành cảm ơn thầy Đường Khánh Sơn đã tận tình hướng dẫn và
giúp đỡ em trong quá trình hoàn thành luận văn. Đồng thời, em cũng gửi lời cảm ơn
chân thành đến các thầy cô trong khoa Kỹ thuật cơ khí, gia đình và bạn bè đã giúp đỡ
và tạo điều kiện tốt nhất cho em trong quá trình học tập cũng như trong quá trình hoàn
thành luận văn tốt nghiệp.
Em xin chân thành cảm ơn!
SVTH: Phan Phúc Hậu Trang I
LỜI CẢM ƠN
LỜI CAM ĐOAN
Em xin cam đoan đề tài: “Thiết kế và chế tạo mô hình Robot SCARA phân
loại sản phẩm theo màu sắc và hình dạng” là một công trình nghiên cứu được thực
hiện bởi cá nhân em dưới sự hướng dẫn của thầy Đường Khánh Sơn. Trong bài báo
cáo, tất cả các số liệu, hình ảnh và thông tin trong đề tài này đều được em thu thập
với một thái độ khách quan và trung thực. Ngoài ra, một số nguồn tài liệu tham khảo
đã được trích dẫn nguồn và chú thích rõ ràng, không sao chép bất cứ tài liệu nào mà
không có trích dẫn. Em xin hoàn toàn chịu trách nhiệm trước bộ môn, khoa và nhà
trường về sự cam đoan này.
Sinh viên thực hiện
SVTH: Phan Phúc Hậu Trang II
TÓM TẮT LUẬN VĂN
TÓM TẮT LUẬN VĂN
Đề tài luận văn “Thiết kế và chế tạo mô hình Robot Scara phân loại sản phẩm
theo màu sắc và hình dạng” bao gồm các nội dung cốt lõi như: tìm hiểu các khái
niệm và kiến thức cơ bản về Robot, thiết kế và chế tạo mô hình Robot Scara 3 bậc tự
do hoạt động linh hoạt và chính xác, thiết kế mạch điều khiển và giao diện điều khiển
cho Robot, xây dựng giải thuật PID điều khiển vị trí động cơ DC, ứng dụng xử lý ảnh
vào mô hình để phân loại sản phẩm.
SVTH: Phan Phúc Hậu Trang III
MỤC LỤC
MỤC LỤC
LỜI CẢM ƠN ............................................................................................................ I
LỜI CAM ĐOAN ..................................................................................................... II
TÓM TẮT LUẬN VĂN .......................................................................................... III
MỤC LỤC ................................................................................................................... i
DANH MỤC HÌNH ẢNH ........................................................................................ iv
DANH MỤC BẢNG ................................................................................................. iv
CHƯƠNG 1: TỔNG QUAN ..................................................................................... 2
1.1 Tổng quan đề tài ........................................................................................ 2
1.1.1 Đặt vấn đề ........................................................................................... 2
1.1.2 Mục tiêu đề tài .................................................................................... 3
1.1.3 Giới hạn đề tài .................................................................................... 3
1.2 Giới thiệu sơ lượt về Robot công nghiệp .................................................. 4
1.2.1 Lịch sử hình thành và phát triển ......................................................... 4
1.2.2 Cấu trúc cơ bản của Robot công nghiệp ............................................. 5
1.2.3 Ứng dụng .......................................................................................... 10
1.3 Giới thiệu sơ lượt về Robot Scara ........................................................... 11
CHƯƠNG 2: ĐỐI TƯỢNG VÀ PHƯƠNG PHÁP NGHIÊN CỨU ................... 14
2.1 Đối tượng nghiên cứu ............................................................................. 14
2.2 Phương pháp nghiêm cứu ....................................................................... 14
2.2.1 Phần mềmMatlab .............................................................................. 14
2.2.2 Matlab GUI ....................................................................................... 17
2.2.3 Xử lý ảnh trên Matlab ....................................................................... 21
2.2.4 Phần mềm Invertor ........................................................................... 27
SVTH: Phan Phúc Hậu Trang i
MỤC LỤC
2.2.5 Arduino ............................................................................................. 29
2.2.6 Phần mềm Arduino IDE ................................................................... 32
2.2.7 PID .................................................................................................... 34
2.2.8 Chuẩn giao tiếp I2C .......................................................................... 37
CHƯƠNG 3: THIẾT KẾ VÀ CHẾ TẠO MÔ HÌNH .......................................... 41
3.1 Bài toán động học ................................................................................... 41
3.1.1 Động học thuận ................................................................................. 41
3.1.2 Động học ngược ............................................................................... 42
3.2 Thiết kế mô hình cơ khí cho Robot ......................................................... 43
3.2.1 Cơ sở thiết kế .................................................................................... 43
3.2.2 Bản vẽ thiết kế Robot trên Inventor ................................................. 44
3.2.3 Chế tạo mô hình cơ khí ..................................................................... 50
3.2.4 Lắp ráp mô hình Robot Scara hoàn chỉnh ........................................ 55
3.3 Thiết kế mạch điều khiển ........................................................................ 57
3.3.1 Lưu đồ giải thuật điều khiển Robot .................................................. 57
3.3.2 Sơ đồ mạch điều khiển Robot ........................................................... 58
CHƯƠNG 4: XỬ LÝ ẢNH VÀ THIẾT KẾ GIAO DIỆN NGƯỜI DÙNG ....... 66
4.1 Xử lý ảnh ................................................................................................. 66
4.1.1 Giới thiệu về hệ thống xử lý ảnh ...................................................... 66
4.1.2 Sơ lượt về quá trình xử lý ảnh trong đề tài ....................................... 68
4.1.3 Thu nhận ảnh đầu vào ....................................................................... 69
4.1.4 Xác định vùng hoạt động của Robot ................................................ 70
4.1.5 Phân vùng ảnh và tách sản phẩm ...................................................... 72
4.1.6 Nhận dạng đối tượng ........................................................................ 74
SVTH: Phan Phúc Hậu Trang ii
MỤC LỤC
4.1.7 Kết quả của quá trình xử lý ảnh ........................................................ 77
4.2 Thiết kế giao diện người dùng ................................................................ 77
CHƯƠNG 5: KẾT QUẢ VÀ HƯỚNG PHÁT TRIỂN ........................................ 79
5.1 Kết quả .................................................................................................... 79
5.1.1 Kết quả mô phỏng trên Matlab Simulink ......................................... 79
5.1.2 Kết quả thực tế của mô hình Robot Scara ........................................ 80
5.1.3 Kết quả chạy thực tế của mô hình Robot Scara ................................ 83
5.2 Đánh giá .................................................................................................. 90
5.3 Kết luận ................................................................................................... 90
5.3.1 Kết quả đạt được ............................................................................... 91
5.3.2 Những hạn chế của đề tài ................................................................. 91
5.4 Hướng phát triển ..................................................................................... 91
TÀI LIỆU THAM KHẢO ...................................................................................... 93
PHỤ LỤC ................................................................................................................. 94
SVTH: Phan Phúc Hậu Trang iii
DANH MỤC HÌNH ẢNH
DANH MỤC HÌNH ẢNH
Hình 1.1: Dây chuyền sản xuất ô tô tự động bằng Robot ........................................... 2
Hình 1.2: Robot Scara phân loại sản phẩm ................................................................. 3
Hình 1.3: Cánh tay Robot 5 bậc tự do của hãng YASKAWA MOTOMAN .............. 4
Hình 1.4: Sơ đồ khối của Robot công nghiệp ............................................................. 5
Hình 1.5: Các thành phần chính của hệ thống Robot.................................................. 6
Hình 1.6: Phân loại Robot theo hình dạng hình học của không gian hoạt động ......... 8
Hình 1.7: Sơ đồ điều khiển hở .................................................................................... 9
Hình 1.8: Sơ đồ điều khiển kín ................................................................................... 9
Hình 1.9: Robot xếp các hàng thùng hàng lên pallet ................................................ 10
Hình 1.10: Robot phun sơn tự động trong sản xuất ô tô ........................................... 10
Hình 1.11: Robot hàn Panasonic ............................................................................... 11
Hình 1.12: Ứng dụng của Robot trong một số lĩnh vực ............................................ 11
Hình 1.13: Robot Scara ............................................................................................. 12
Hình 1.14: Robot SCARA của hãng MITSUBISHI ................................................. 12
Hình 1.15: Robot SCARA trong dây chuyền sản xuất linh hoạt .............................. 13
Hình 2.1: Phần mềm Matlab ..................................................................................... 15
Hình 2.2; Giao diện làm việc của Matlab ................................................................. 16
Hình 2.3: Cửa sổ GUIDE Quick Start ....................................................................... 18
Hình 2.4: Giao diện Blank GUI mặc định ................................................................ 18
Hình 2.5: Hộp thoại Inspector ................................................................................... 20
Hình 2.6: Ảnh Index .................................................................................................. 22
Hình 2.7: Ảnh xám .................................................................................................... 23
Hình 2.8: Ảnh nhị phân ............................................................................................. 23
Hình 2.9: Ảnh RGB ................................................................................................... 24
Hình 2.10: Arduino Mega 2560 và Arduino Uno R3 ............................................... 29
Hình 2.11: Phần cứng của Arduino Uno R3 ............................................................. 30
Hình 2.12: Ba loại Arduino phổ biến ........................................................................ 31
Hình 2.13: Một số loại Arduino nâng cao ................................................................. 31
Hình 2.14: Phần mềm Arduino IDE .......................................................................... 32
SVTH: Phan Phúc Hậu Trang iv
DANH MỤC HÌNH ẢNH
Hình 2.15: Một chương trình Arduino cơ bản .......................................................... 33
Hình 2.16: Giao diện của phần mềm Arduino IDE ................................................... 33
Hình 2.17: Ba thành phần của bộ điều khiển PID .................................................... 35
Hình 2.18: Mô hình hệ thống điều khiển với bộ PID................................................ 35
Hình 2.19: Sơ đồ truyền dữ liệu I2C ......................................................................... 38
Hình 2.20: Điều kiện START và STOP .................................................................... 38
Hình 2.21: Điều kiện REPEATED START .............................................................. 39
Hình 2.22: Giao thức truyền nhận dữ liệu I2C .......................................................... 39
Hình 3.1: Gắn hệ tọa độ cho Robot ........................................................................... 41
Hình 3.2: Sắt tấm ....................................................................................................... 50
Hình 3.3: Sắt hộp ....................................................................................................... 50
Hình 3.4: Nhôm tấm .................................................................................................. 50
Hình 3.5: Động cơ M555 .......................................................................................... 51
Hình 3.6: Động cơ Planet .......................................................................................... 52
Hình 3.7: Bộ truyền Pully và dây đai ........................................................................ 52
Hình 3.8: Bộ thanh trục vít-me đai ốc ....................................................................... 53
Hình 3.9: Bộ thanh trượt, ray trượt và gối đỡ ........................................................... 54
Hình 3.10: Bạc đạn đỡ trục KFL08 ........................................................................... 54
Hình 3.11: Vòng bi trục ............................................................................................ 54
Hình 3.12: Khớp nối mặt bích cố định ...................................................................... 54
Hình 3.13: Bulong đai ốc .......................................................................................... 54
Hình 3.14: Phần đế thực tế của mô hình Robot Scara .............................................. 55
Hình 3.15: Khâu tịnh tiến thực tế của mô hình Robot Scara .................................... 55
Hình 3.16: Hai khâu quay thực tế của mô hình Robot Scara .................................... 56
Hình 3.17: Mô hình cơ khí hoàn thiện của Robot Scara ........................................... 56
Hình 3.18: Lưu đồ giải thuật điều khiển ................................................................... 57
Hình 3.19: Mạch điều khiển Robot ........................................................................... 58
Hình 3.20: Sơ đồ chân của Arduino Uno .................................................................. 59
Hình 3.21: Sơ đồ chân của Arduino Nano ................................................................ 61
Hình 3.22: Sơ đồ chân của Module L298N .............................................................. 63
SVTH: Phan Phúc Hậu Trang v
DANH MỤC HÌNH ẢNH
Hình 3.23: Nguồn tổ ong 12V-10A .......................................................................... 64
Hình 3.24: Động cơ Servo MG995 ........................................................................... 65
Hình 4.1: Quá trình xử lý ảnh ................................................................................... 66
Hình 4.2: Các bước cơ bản trong xử lý ảnh .............................................................. 67
Hình 4.3: Sơ đồ quá trình xử lý ảnh trong đề tài ....................................................... 69
Hình 4.4: Webcam logitech HD C270 ...................................................................... 69
Hình 4.5: Kích thước vùng hoạt động ....................................................................... 70
Hình 4.6: Ảnh quá trình xác định vùng hoạt động của Robot .................................. 70
Hình 4.7: Sơ đồ quá trình tìm vùng hoạt động của Robot ........................................ 71
Hình 4.8: Vùng hoạt động của Robot ....................................................................... 71
Hình 4.9: Sơ đồ quá trình nhị phân ảnh .................................................................... 72
Hình 4.10: Ảnh quá trình nhị phân ảnh ..................................................................... 72
Hình 4.11: Sơ đồ giải thuật phân vùng và tách đối tượng ........................................ 73
Hình 4.12: Ảnh kết quả của quá trình phân vùng ..................................................... 73
Hình 4.13: Giải thuật quá trình nhận dạng màu sắc .................................................. 74
Hình 4.14: Ảnh nhị phân từng màu của vật .............................................................. 75
Hình 4.15: Ảnh kết quả của quá trình nhận dạng màu sắc........................................ 75
Hình 4.16: Giải thuật quá trình nhận dạng hình dạng ............................................... 76
Hình 4.17: Ảnh kết quả của quá trình nhận dạng hình dạng ..................................... 76
Hình 4.18: Ảnh kết quả của quá trình nhận dạng màu sắc và hình dạng .................. 77
Hình 4.19: Giao diện người dùng điều khiển Robot Scara ....................................... 78
Hình 5.1: Mô hình hóa Robot Scara trên Matlab Simulink ...................................... 79
Hình 5.2: Mô hình 3D khi mô phỏng trên Matlab Simulink .................................... 79
Hình 5.3: Kết quả chạy thực nghiệm PID trên khớp 1 .............................................. 80
Hình 5.4: Kết quả chạy thực nghiệm PID trên khớp 2 .............................................. 80
Hình 5.5: Kết quả chạy thực nghiệm PID trên khâu 3 .............................................. 81
Hình 5.6: Tay gắp thực hiện gắp vật ......................................................................... 81
Hình 5.7: Tay gắp thực hiện thả vật .......................................................................... 81
Hình 5.8: Kết quả chạy thực tế động học thuận của Robot Scara............................. 82
Hình 5.9: Kết quả chạy thực tế động học ngược của Robot Scara ........................... 82
SVTH: Phan Phúc Hậu Trang vi
DANH MỤC HÌNH ẢNH
Hình 5.10: Kết quả nhận dạng màu sắc các vật hình tròn ......................................... 83
Hình 5.11: Quá trình phân loại màu sắc các vật hình tròn của Robot Scara ............ 84
Hình 5.12: Kết quả nhận dạng màu sắc các vật hình vuông ..................................... 84
Hình 5.13: Quá trình phân loại màu sắc các vật hình vuông của Robot Scara ......... 85
Hình 5.14: Kết quả nhận dạng hình dạng các vật màu đỏ ........................................ 85
Hình 5.15: Quá trình phân loại hình dạng các vật màu đỏ của Robot Scara ............ 86
Hình 5.16: Kết quả nhận dạng hình dạng các vật màu xanh lá ................................. 86
Hình 5.17: Quá trình phân loại hình dạng các vật màu xanh lá của Robot Scara..... 87
Hình 5.18: Kết quả nhận dạng hình dạng các vật màu xanh dương ......................... 87
Hình 5.19: Quá trình phân loại hình dạng các vật màu xanh dương của Robot ....... 88
Hình 5.20: Kết quả nhận dạng màu sắc và hình dạng của tất cả các vật .................. 88
Hình 5.21: Quá trình phân loại màu sắc và hình dạng của Robot Scara ................... 90
SVTH: Phan Phúc Hậu Trang vii
DANH MỤC BẢNG
DANH MỤC BẢNG
Bảng 2.1: Bảng thông số PID theo phương pháp Ziegler-Nichols ........................... 37
Bảng 3.1: Bảng thông số DH .................................................................................... 41
Bảng 3.2: Bảng thông số động cơ ............................................................................. 52
Bảng 3.3: Bảng thông số bộ truyền ........................................................................... 53
SVTH: Phan Phúc Hậu Trang iv
LỜI NÓI ĐẦU
LỜI NÓI ĐẦU
Cùng với sự phát triển của các cuộc cách mạng khoa học kỹ thuật trên thế giới
là những tiến bộ trong lĩnh vực điều khiển và tự động hoá sản xuất. Một trong những
thành tựu nổi bật của quá trình tự động hoá là những cánh tay máy Robot. Những
Robot từ đơn giản đến phức tạp, đã đóng góp rất nhiều trong việc thay thế con người
ở môi trường làm việc cường độ cao, độc hại và đòi hỏi chính xác cao.
Để có thể phát triển và ứng dụng Robot rộng rãi trong sản xuất, bên cạnh những
kiến thức về điện, điện tửthì việc nghiên cứu tính toán động học và đ...À PHƯƠNG PHÁP NGHIÊM CỨU
Ảnh grayscale
Mỗi ảnh được biểu diễn bởi một ma trận hai chiều, trong đó giá trị của mỗi phần
tử cho biết độ sáng (hay mức xám) của điểm ảnh đó. Ma trận này có thể có một trong
các kiểu dữ liệu uint8, uint16 hoặc double. Ảnh biểu diễn theo kiểu này còn gọi là
ảnh “trắng đen”.
Hình 2.7: Ảnh xám
Ảnh nhị phân:
Ảnh được biểu diễn bởi một ma trận hai chiều thuộc kiểu logical. Mỗi điểm ảnh
chỉ có thể nhận một trong hai giá trị là 0 (đen) hoặc 1 (trắng).
Hình 2.8: Ảnh nhị phân
SVTH: Phan Phúc Hậu Trang 23
CHƯƠNG 2: ĐỐI TƯỢNG VÀ PHƯƠNG PHÁP NGHIÊM CỨU
Ảnh RGB
Còn gọi là ảnh “truecolor” do tính trung thực của nó. Ảnh này được biểu diễn
bởi một ma trận ba chiều kích thước m x n x 3, với m x n là kích thước ảnh theo
pixels. Ma trận này định nghĩa các thành phần màu red, green, blue cho mỗi điểm
ảnh, các phần tử của nó có thể thuộc kiểu uint8, uint16 hoặc double.
Hình 2.9: Ảnh RGB
2.2.3.2.1 Các hàm xử lý cơ bản
- Hàm imread đọc các file ảnh với bất kỳ các định dạng ảnh đã biết hiện nay và
lưu lại dưới dạng một ma trận biểu diễn ảnh trong Matlab.
Cú pháp: A=imread(filename,fmt).
- Hàm imwrite cho phép lưu một ảnh biểu diễn bằng một ma trận trong Matlab
thành một file ảnh dưới một trong các định dạng đã biết.
Cú pháp: imwrite(A,filename,fmt).
- Hàm imfinfo dùng để xem các thông số của một file ảnh nào đó.
Cú pháp : imfinfo(filename,fmt).
Các thông tin được cung cấp bởi hàm imfinfo là : filename, filemoddate,
filesize, format, formatversion, width, height, bitdepth, colortype.
2.2.3.2.2 Chuyển đổi giữa các kiểu dữ liệu, kiểu ảnh
Chuyển đổi giữa các kiểu dữ liệu ảnh
SVTH: Phan Phúc Hậu Trang 24
CHƯƠNG 2: ĐỐI TƯỢNG VÀ PHƯƠNG PHÁP NGHIÊM CỨU
Matlab cung cấp sẵn các hàm thực hiện chuyển kiểu cho các ma trận biểu diễn
ảnh, bao gồm : im2double, im2uint8 và im2uint16. Tuy nhiên, khi thực hiện chuyển
kiểu giữa các dữ liệu ảnh cần lưu ý một số điều sau:
- Khi chuyển từ một kiểu dữ liệu dùng nhiều bit sang một kiểu dữ liệu dùng ít bit
hơn thì một số thông tin chi tiết về bức ảnh ban đầu sẽ bị mất.
- Không phải lúc nào cũng có thể chuyển đổi kiểu dữ liệu đối với kiểu ảnh
indexed, vì các giá trị của ma trận ảnh xác định một địa chỉ trong bản đồ màu chứ
không phải là giá trị màu, do đó không thể lượng tử hóa được.
Chuyển đổi giữa các kiểu ảnh
- Dither: Tạo một ảnh nhị phân từ một ảnh cường độ đen trắng bằng cách trộn,
tạo một ảnh chỉ số từ một ảnh RGB bằng cách trộng (dither).
Cú pháp: dither(RGB,map); dither(I)
- Gray2id: Tạo một ảnh chỉ số từ một ảnh cường độ đen trắng.
Cú pháp: [X,Map] = gray2ind(I,N); [X,Map] = gray2ind(BW,N)
- Grayslice: Tạo một ảnh chỉ số từ một ảnh cường độ đen trắng bằng cách đặt
ngưỡng.
Cú pháp: x=grayslice(I,N); x=grayslice(I,V)
- Im2bw: Tạo một ảnh nhị phân từ một ảnh cường độ , ảnh chỉ số hay ảnh.
Cú pháp: bw=im2bw(I,level); bw=im2bw(x,map,level); bw=im2bw(rgb,level)
- Ind2gray: Tạo một ảnh cường độ đen trắng từ một ảnh chỉ số.
Cú pháp: i=ind2gray(x,map)
- Ind2rgb: Tạo một ảnh RGB từ một ảnh chỉ số.
Cú pháp: rgb=ind2rgb(x,map)
- Mat2gray: Tạo một ảnh cường độ đen trắng từ dữ liệu trong một ma trận bằng
cách lấy tỉ lệ giữ liệu.
Cú pháp: i=mat2gray(a,[amin amax])
- Rgb2gray: Tạo một ảnh cường độ đen trắng từ một ảnh RGB.
Cú pháp: i=rgb2gray(rgb)
- Rgb2ind: Tạo một ảnh chỉ số từ một ảnh RGB.
Cú pháp: i=rgb2gray(rgb); x=rgb2ind(rgb,map); [x,map]=rgb2ind(rgb,tol)
SVTH: Phan Phúc Hậu Trang 25
CHƯƠNG 2: ĐỐI TƯỢNG VÀ PHƯƠNG PHÁP NGHIÊM CỨU
Ta cũng có thể thực hiện các phép chuyển đổi kiểu chỉ sử dụng cú pháp của
Matlab. Chẳng hạn, ta có thể convert một ảnh cường độ sang ảnh RGB bằng cách
ghép nối 3 phần copy của ma trận ảnh gốc giữa 3 chiều: RGB= cat (3,I,I,I). Ảnh
RGB thu được có các ma trận đồng nhất cho các mặt phẳng R,G,B vì vậy ảnh hiển
thị giống như bóng xám.
2.2.3.3 Các hàm hiển thị trong Matlab
Matlab cung cấp hai hàm hiển thị cơ bản là image và imagesc. Ngoài ra trong
IPT cũng có hai hàm hiển thị ảnh khác, đó là imview và imshow.
- Hàm image(X,Y,C) hiển thị hình ảnh biểu diễn bởi ma trận C kích thước m x n
lên trục tọa độ hiện hành. X, Y là các vector xác định vị trí các pixel C(1,1) và C(m,n)
trong hệ trục hiện hành.
- Hàm imagesc có chức năng tương tự như hàm image, ngoại trừ việc dữ liệu ảnh
sẽ được co giãn để sử dụng toàn bộ bản đồ màu hiện hành.
- Hàm imview cho phép hiển thị hình ảnh trên một cửa sổ riêng, nền Java, gọi là
image Viewer. Image Viewer cung cấp các công cụ dò tìm và xác định các giá trị
pixel một cách linh hoạt.
- Hàm imshow cũng tạo một đối tượng đồ họa thuộc loại image và hiển thị ảnh
trên một figure. Hàm imshow sẽ tự động thiết lập các giá trị của các đối tượng image,
axes và figure để thể hiện hình ảnh.
2.2.3.3.1 Các phép biến đổi hình học
Thay đổi kích thước ảnh
Hàm imresize cho phép người sử dụng thay đổi kích thước ảnh. Ngoài kích
thước ảnh mới, người sử dụng còn có thể xác định phương pháp nội suy sẽ dùng và
loại bộ lọc dùng để chống aliasing.
Cú pháp: b=imresize(a,m, method): tạo ảnh gấp m lần ảnh a.
b=imresize(a,[mrows mcols],method)
b=imresize(a,[mrows mcols],method,N)
b=imresize(a,[mrows mcols],method,h)
Phép quay ảnh
SVTH: Phan Phúc Hậu Trang 26
CHƯƠNG 2: ĐỐI TƯỢNG VÀ PHƯƠNG PHÁP NGHIÊM CỨU
Để thực hiện phép quay ảnh, ta có thể sử dụng hàm imrotate. Ngoài hai thông
số cơ bản là ảnh gốc và góc quay, người sử dụng cũng có thể xác định phương pháp
nội suy sẽ dùng và kích thước của ảnh mới.
Cú pháp: b=imrotate(a,angle,method,Bbox)
Trích xuất ảnh
Khi cần trích xuất một phần ảnh gốc, ta dùng hàm imcrop.
- Xác định cụ thể vị trí của phần ảnh cần trích xuất (dưới dạng hình chữ nhật).
Cú pháp: x2=imcrop(x,map, [Xmin Ymin width height])
x2=imcrop(a, [Xmin Ymin width height])
- Sử dụng mouse để chọn phần ảnh cần trích xuất. Ta không cần cung cấp thông
số rect, khi thực hiện hàm này, con trỏ sẽ chuyển sang dạng chữ thập, người dùng sẽ
kéo chuột để chọn phần ảnh cần trích xuất sau đó thả chuột.
2.2.4 Phần mềm Invertor
2.2.4.1 Giới thiệu phần mềm Inventor
Autodesk Inventor được phát triển bởi công ty phần mềm Autodesk_USA, là
phần mềm thiết kế mô hình 3D phổ biến hiện nay. Đây là phần mềm được phát triển
chuyên cho thiết kế các sản phẩm cơ khí, có giao diện trực quan, giúp người dùng
thuận tiện khi sử dụng với những tính năng nổi trội như sau:
- Xây dựng dễ dàng mô hình 3D của chi tiết (Part).
- Thiết lập các bản 2D từ mô hình 3D nhanh chóng và chuẩn xác (Drawing).
- Tạo bản vẽ lắp từ các chi tiết đã thiết kế một cách tối ưu (Assembly).
- Mô phỏng quá trình tháo lắp các chi tiết từ bản vẽ lắp hoàn chỉnh một cách trực
quan và sinh động (Presentation).
- Thiết kế nhanh các chi tiết kim loại dạng tấm (Sheet metal).
- Thiết kế các chi tiết máy như: Trục, bộ truyền bánh răng, bộ truyền đai, bộ truyền
xích, mối ghép bulông-đai ốc, cam, chốt, then, ổ bi, lò xo, một cách nhanh chóng
trong môi trường Assembly.
- Thiết kế nhanh và chính xác các loại khuôn mẫu (Mold Design).
- Thiết kế nhanh các đường ống phức tạp (Pipe&Tupe).
SVTH: Phan Phúc Hậu Trang 27
CHƯƠNG 2: ĐỐI TƯỢNG VÀ PHƯƠNG PHÁP NGHIÊM CỨU
- Cho phép sử dụng thư viện các loại dây điện và cáp điện để chạy dây với bán
kính uốn phù hợp trong thiết kế điện (Cable &Wiring):.
- Mô phỏng động và động lực học của cơ cấu máy (Dynamic simulation).
- Phân tích ứng suất, tối ưu hóa thiết kế sản phẩm (Analysis Stress and Optimize).
- Thiết kế nhanh các sản phẩm nhựa (Inventor plastic & tooling).
- Có thư viện chi tiết đa dạng và chuẩn hóa (Content center).
- Liên kết được với nhiều phần mềm CAD khác.
2.2.4.2 Liên kết Inventor với công cụ simscape multibody của Matlab
Bước 1: Nhận tập tin cài đặt
- Vào trang tải xuống Simscape Multibody Link.
- Làm theo hướng dẫn trên trang tải xuống.
- Lưu tệp .zip và tệp Matlab trong thư mục thuận tiện.
Lưu ý: Chọn phiên bản khớp với số phát hành Matlab và kiến trúc hệ thống của
bạn. Không giải nén tệp lưu trữ zip.
Bước 2: Chạy chức năng cài đặt
- Chạy Matlab với quyền administrator.
- Thêm các tệp đã cài đặt vào đường dẫn Matlab.
- Tại dấu nhắc lệnh Matlab ta nhập “install_addon(‘zipname’)”.
Trong đó: “zipname” là tên của tệp lưu trữ zip.
Bước 3: Đăng ký Matlab làm máy chủ tự động
Mỗi khi ta xuất mô hình lắp ráp CAD thì Simscape Multibody Link sẽ cố gắng
kết nối với Matlab. Để kết nối được xảy ra thì bạn phải đăng ký Matlab làm máy chủ
tự động. Bạn có thể làm điều này theo hai cách sau:
- Trong cửa sổ Matlab đang chạy với quyền administrator thì tại dấu nhắc lệnh ta
nhập “regMatlabserver”.
- Trong cửa sổ MS-DOS đang chạy với quyền administrator thì tại dấu nhắc lệnh
ta nhập “Matlab-regserver”.
Bước 4: Kích hoạt Simscape Multibody Link
Trước khi ta có thể xuất một cụm thì ta cần phải kích hoạt Simscape Multibody
Link trên ứng dụng Invertor. Để làm điều này thì tại dấu nhắc lệnh ta nhập
SVTH: Phan Phúc Hậu Trang 28
CHƯƠNG 2: ĐỐI TƯỢNG VÀ PHƯƠNG PHÁP NGHIÊM CỨU
“smlink_linkinv”. Khi đó trong lần đầu tiên ta mở Invertor thì menu Simscape
Multibody Link sẽ xuất hiện.
Bước 5: Xuất file Inventor sang Matlab
- Trong cửa sổ Inventor ta chọn Add-Ins sau đó chọn vào Export Simscape
Multibody First Generation(1G) để xuất ra file *.xml
- Trong cửa sổ Matlab đang chạy với quyền administrator
- Thêm file *.xml đã xuất vào đường dẫn của Matlab.
- Tại dấu nhắc lệnh ta nhập “smimport(‘xmlname’)”.
Trong đó: “xmlname” là tên của file *.xml
2.2.5 Arduino
2.2.5.1 Arduino là gì ?
Arduino là một bo mạch vi điều khiển do một nhóm giáo sư và sinh viên Ý thiết
kế và đưa ra đầu tiên vào năm 2005. Mạch Arduino được sử dụng để cảm nhận và
điều khiển nhiều đối tượng khác nhau. Nó có thể thực hiện nhiều nhiệm vụ từ lấy tín
hiệu từ cảm biến đến điều khiển đèn, động cơ, và nhiều đối tượng khác. Ngoài ra
mạch còn có khả năng liên kết với nhiều module khác nhau như module đọc thẻ từ,
ethernet shield, sim900A, .để tăng khả ứng dụng của mạch.
Phần cứng bao gồm một board mạch nguồn mở được thiết kế trên nền tảng vi
xử lý AVR Atmel 8bit, hoặc ARM, Atmel 32-bit, Hiện phần cứng của Arduino có
tất cả 6 phiên bản, tuy nhiên phiên bản thường được sử dụng nhiều nhất là Arduino
Uno và Arduino Mega. Arduino là vi điều khiển được sử dụng rộng rãi trên thế giới
với những ưu điểm như rẻ, tương thích được với nhiều hệ điều hành, chương trình
lập trình đơn giản, rõ ràng, dễ sử dụng, sử dụng mã nguồn mở và có thể kết hợp với
nhiều module khác nhau.
Hình 2.10: Arduino Mega 2560 và Arduino Uno R3
SVTH: Phan Phúc Hậu Trang 29
CHƯƠNG 2: ĐỐI TƯỢNG VÀ PHƯƠNG PHÁP NGHIÊM CỨU
2.2.5.2 Phần cứng của Arduino
Hình 2.11: Phần cứng của Arduino Uno R3
Mỗi loại Arduino sẽ có cấu hình phần cứng khác nhau, tuy nhiên nó vẫn có đầy
đủ các phần cơ bản như trên Arduino UNO R3, gồm có 6 phần cơ bản:
1. Cổng USB (loại B): Đây là cổng giao tiếp để ta upload code từ máy tính lên vi
điều khiển. Đồng thời nó cũng là giao tiếp serial để truyền dữ liệu giữa vi điểu khiển
với máy tính.
2. Jack nguồn: Để chạy Arduino thì có thể lấy nguồn từ cổng USB ở trên, nhưng
không phải lúc nào cũng có thể cắm với máy tính được. Lúc đó, ta cần một nguồn từ
9V đến 12V.
3. Hàng header thứ nhất: Đánh số từ 0 đến 12 là hàng digital pin, nhận vào hoặc
xuất ra các tín hiệu số. Ngoài ra có một pin mass (GND) và pin điện áp tham chiếu
(AREF).
4. Hàng header thứ hai: Chủ yếu liên quan đến điện áp mass (GND), nguồn.
5. Hàng header thứ ba: Các chân để nhận vào hoặc xuất ra các tín hiệu analog.
Ví dụ như đọc thông tin của các thiết bị cảm biến.
6. Vi điều khiển AVR: Đây là bộ xử lý trung tâm của toàn bo mạch. Với mỗi mẫu
Arduino khác nhau thì con chip này khác nhau. Ở con Arduino Uno này thì sử dụng
ATMega328.
Đối với các Arduino khác tùy vào kích thước và phần cứng sẽ có cách sắp xếp
linh kiện khác nhau, tuy nhiên vẫn giữ cấu hình cơ bản như Arduino UNO R3, một
số Arduino có kích thước nhỏ sẽ không có jack cấp nguồn DC.
SVTH: Phan Phúc Hậu Trang 30
CHƯƠNG 2: ĐỐI TƯỢNG VÀ PHƯƠNG PHÁP NGHIÊM CỨU
2.2.5.3 Một số loại Arduino điển hình
Arduino gồm rất nhiều phiên bản với các cấu hình khác nhau nhưng hầu như
vẫn đảm nhận được các chức năng cơ bản tương đương nhau.
Ba loại Arduino phổ biến nhất là Arduino UNO R3, Nano, Mega 2560:
Hình 2.12: Ba loại Arduino phổ biến
Bên cạnh đó còn nhiều loại Arduino được nâng cao hơn nhắm đáp ứng nhu
cầu sử dụng cao hơn trong công nghiệp như: Arduino Yun, Arduino Ethernet,
Arduino Tre, Arduino Robot,
Hình 2.13: Một số loại Arduino nâng cao
2.2.5.4 Ứng dụng của Arduino
Arduino có nhiều ứng dụng trong đời sống, trong việc chế tạo các thiết bị điện
tử chất lượng cao. Một số ứng dụng có thể kể đến như:
SVTH: Phan Phúc Hậu Trang 31
CHƯƠNG 2: ĐỐI TƯỢNG VÀ PHƯƠNG PHÁP NGHIÊM CỨU
- Lập trình Robot: Arduino chính là một phần quan trọng trong trung tâm xử lý
giúp điều khiển được hoạt động của Robot.
- Lập trình máy bay không người lái. Có thể nói đây là ứng dụng có nhiều kì vọng
trong tương lai.
- Game tương tác: Chúng ta có thể dùng Arduino để tương tác với Joystick, màn
hình,... để chơi các trò như Tetrix, phá gạch, Mario,... và nhiều game rất sáng tạo nữa.
- Arduino cũng được ứng dụng trong máy in 3D và nhiều ứng dụng khác tùy
thuộc vào khả năng sáng tạo của người sử dụng.
Trong đề tài, sử dụng 1 board Arduino UNO R3 (Master) làm trung tâm truyền
tính hiệu điều khiển và 3 board Arduino Nano (Slave) làm nhiệm vụ điều khiển trực
tiếp 3 động cơ của tay máy. Có thể nói rằng Arduino đảm nhiệm vai trò là bộ xử lí
trung tâm cho toàn bộ Robot, vừa đảm nhiệm vai trò nhận tín hiệu điều khiển từ máy
tính vừa xử lí và xuất tín hiệu để động cơ hoạt động.
2.2.6 Phần mềm Arduino IDE
2.2.6.1 Phần mềm Arduino IDE là gì?
Arduino IDE được viết tắt (Arduino Integrated Development Environment) là
một trình soạn thảo văn bản, giúp bạn viết code để nạp vào bo mạch Arduino.
Hình 2.14: Phần mềm Arduino IDE
Arduino IDE được tạo ra bởi ngôn ngữ lập trình Java chính là ứng dụng đa
nền tảng (cross-platform). Ngôn ngữ code sử dụng cho các chương trình trong
Arduino là C hoặc C++. Bản thân Arduino ide đã được trang bị một thư viện phần
mềm thường gọi là “wiring”, từ các chương trình “wiring” gốc đó sẽ giúp các bạn
thực hiện thao tác code dễ dàng và nhanh hơn. Một chương trình chạy trong Arduino
được gọi là một sketch, chương trình được định dạng dưới dạng .ino .
SVTH: Phan Phúc Hậu Trang 32
CHƯƠNG 2: ĐỐI TƯỢNG VÀ PHƯƠNG PHÁP NGHIÊM CỨU
2.2.6.2 Cấu trúc chương cơ bản trong Arduino IDE
Những lệnh trong setup() sẽ được chạy khi chương trình của bạn khởi động.
Bạn có thể sử dụng nó để khai báo giá trị của biến, khai báo thư viện, thiết lập các
thông số, Sau khi setup() chạy xong, những lệnh trong loop() được chạy. Chúng sẽ
lặp đi lặp lại liên tục cho tới khi nào bạn ngắt nguồn của board Arduino mới thôi. Bất
cứ khi nào bạn nhất nút Reset, chương trình của bạn sẽ trở về lại trạng thái như khi
Arduino mới được cấp nguồn. Quá trình này có thể được miêu tả như sơ đồ dưới đây:
Hình 2.15: Một chương trình Arduino cơ bản
2.2.6.3 Giao diện Arduino IDE
Hình 2.16: Giao diện của phần mềm Arduino IDE
SVTH: Phan Phúc Hậu Trang 33
CHƯƠNG 2: ĐỐI TƯỢNG VÀ PHƯƠNG PHÁP NGHIÊM CỨU
Giao diện của phần mềm Arduino IDE có nhiều phần, tuy nhiên chúng ta chú
ý đến những phần quan trọng như:
1. Nút kiểm tra chương trình
Dùng để kiểm tra xem chương trình được viết có lỗi không. Nếu chương trình bị
lỗi thì phần mềm Arduino IDE sẽ hiển thị thông tin lỗi ở vùng thông báo thông tin.
2. Nút nạp chương trình xuống bo Arduino
Dùng để nạp chương trình được viết xuống mạch Arduino. Trong quá trình nạp,
chương trình sẽ được kiểm tra lỗi trước sau đó mới thực hiện nạp xuống mạch
Arduino.
3. Hiển thị màn hình giao tiếp với máy tính
Khi nhấp vào biểu tượng cái kính lúp thì phần giao tiếp với máy tính sẽ được mở
ra. Phần này sẽ hiển thị các thông số mà người dùng muốn đưa lên màn hình. Muốn
đưa lên màn hình phải có lệnh Serial.print() mới có thể đưa thông số cần hiển thị lên
màn hình
4. Vùng lập trình
Vùng này để người lập trình thực hiện việc lập trình cho chương trình của mình.
5. Vùng thông báo thông tin:
Có chức năng thông báo các thông tin lỗi của chương trình hoặc các vấn đề liên
quan đến chương trình được lập.
2.2.7 PID
2.2.7.1 Lý thuyết về bộ điều khiển PID
Tên gọi PID là chữ viết tắt của ba thành phần cơ bản trong bộ điều khiển: khuếch
đại tỷ lệ (P), tích phân (I), vi phân (D). Bộ điều khiển PID được ví như một tập thể
hoàn hảo gồm ba cá nhân với ba tính cách khác nhau:
- Phục tùng và thực hiện chính xác nhiệm vụ được giao (tỷ lệ).
- Làm việc và có tích lũy kinh nghiệm để thực hiện tốt nhiệm vụ được giao (tích
phân).
- Luôn có sáng kiến và phản ứng nhanh nhạy với sự thay đổi trong quá trình làm
nhiệm vụ (vi phân).
SVTH: Phan Phúc Hậu Trang 34
CHƯƠNG 2: ĐỐI TƯỢNG VÀ PHƯƠNG PHÁP NGHIÊM CỨU
Hình 2.17: Ba thành phần của bộ điều khiển PID
Bộ điều khiển PID được sử dụng khá rộng rãi để điều khiển đối tượng SISO
(một vào, một ra) theo nguyên lý hồi tiếp. Lý do mà bộ điều khiển PID được sử dụng
rộng rãi là vì tính đơn giản của nó cả về cấu trúc lẫn nguyên lý làm việc. Bộ điều
khiển PID có nhiệm vụ đưa sai lệch e(t) của hệ thống về 0 sao cho quá trình quá độ
thỏa mãn các yêu cầu cơ bản về chất lượng. Nguyên lý làm việc của bộ điều khiển
PID được mô tả một cách định tính như sau:
- Nếu sai lêch e(t) càng lớn thì thông qua thành phần uP(t) tín hiệu điều chỉnh u(t)
càng lớn (vai trò của khâu khuếch đại tỷ lệ KP).
- Nếu sai lệch e(t) chưa bằng 0 thì thông qua thành phần uI(t) bộ điều khiển PID
vẫn còn tạo tín hiệu điều chỉnh u(t) (vai trò của khâu tích phân TI)
- Nếu sự thay đổi của sai lệch e(t) càng lớn thì thông qua thành phần uD(t) bộ điều
khiển PID sẽ phản ứng càng nhanh (vai trò của khâu vi phân TD)
Hình 2.18: Mô hình hệ thống điều khiển với bộ PID
Biểu thức toán học của bộ điều khiển PID có dạng:
1 푡 푑푒(푡)
푢(푡) = 퐾푃 [푒(푡) + ∫ 푒(푡)푑푡 + 푇퐷 ] (2.1)
푇퐼 0 푑푡
Trong đó: e(t) là tín hiệu đầu vào; u(t) là tín hiệu đầu ra
KP là hệ số khuếch đại; TI là hằng số tích phân; TD là hằng số vi phân
SVTH: Phan Phúc Hậu Trang 35
CHƯƠNG 2: ĐỐI TƯỢNG VÀ PHƯƠNG PHÁP NGHIÊM CỨU
Từ biểu thức toán học trên ta suy ra được hàm truyền đạt của bộ điều khiển PID:
1
푅(푠) = 퐾푃 (1 + + 푇퐷푠) (2.2)
푇퐼푠
Chất lượng của hệ thống phụ thuộc vào các tham số KP, TI, TD. Muốn hệ thống
có được chất lượng như mong muốn thì phải phân tích đối tượng rồi trên cơ sở đó
chọn các tham số KP, TI, TD phù hợp. Hiện có khá nhiều phương pháp xác định tham
số KP, TI, TD cho bộ điều khiển PID. Sau đây là một số phương pháp thường được sử
dụng: Phương pháp tối ưu Môđun (phương pháp tối ưu đối xứng), Phương pháp của
Ziegler-Nichols, Phương pháp thực nghiệm, Phương pháp Reinisch.
Một điểm cần chú ý là trong nhiều trường hợp không cần xác định cả ba tham
số KP, TI, TD cho bộ điều khiển PID. Chẳng hạn như bản thân đối tượng đã có thành
phần tích phân thì trong bộ điều khiển ta không cần phải có thêm khâu tích phân mới
triệt tiêu được sai lệch tĩnh, tức là ta chỉ cần sử dụng bộ điều khiển PD là đủ. Nếu tín
hiệu trong hệ thống thay đổi tương đối chậm và bản thân bộ điều khiển không cần
phải có phản ứng thật nhanh với sự thay đổi của sai lệch e(t) thì ta chỉ cần sử dụng bộ
điều khiển PI.
2.2.7.2 Các phương pháp tìm thông số bộ điều khiển PID:
Do từng thành phần của bộ điều khiển PID có những ưu nhược điểm khác nhau,
và không thể đồng thời đạt được tất cả các chỉ tiêu chất lượng một cách tối ưu, nên
cần lựa chọn, thỏa hiệp giữa các yêu cầu chất lượng và mục đích điều khiển. Việc lựa
chọn tham số cho bộ điều khiển PID cũng phụ thuộc vào đối tượng điều khiển và các
phương pháp xác định thông số.
Phương pháp chỉnh định bằng tay
- Đặt KI = KD = 0. Tăng KP đến khi hệ thống dao động tuần hoàn.
- Đặt thời gian tích phân bằng chu kỳ dao động.
- Điều chỉnh lại giá trị KP cho phù hợp.
- Nếu có dao động thì điều chỉnh giá trị KD.
Phương pháp Ziegler-Nichols
- Đặt KI = KD = 0. Tăng KP đến khi hệ thống dao động tuần hoàn.
- Đặt giá trị KP này = KC
SVTH: Phan Phúc Hậu Trang 36
CHƯƠNG 2: ĐỐI TƯỢNG VÀ PHƯƠNG PHÁP NGHIÊM CỨU
- Đo chu kì dao động PC.
Bảng 2.1: Bảng thông số PID theo phương pháp Ziegler-Nichols
Thông số
K K K
Bộ ĐK P I D
P 0.5KC - -
PI 0.45KC 1.2KP/PC -
PID 0.6KC 2KP/PC KPPC/8
Phương pháp chỉnh định bằng phần mềm
- Dùng phần mềm để tự động chỉnh định thông số PID (thực hiện trên mô hình
toán, kiểm nghiệm trên mô hình thực).
- Ví dụ dùng giải thuật di truyền (GA) để tìm thông số sao cho sai số đo được nhỏ
hơn giá trị yêu cầu.
2.2.8 Chuẩn giao tiếp I2C
2.2.8.1 Sơ lượt về I2C
I2C là giao thức truyền tin nối tiếp đồng bộ sử dụng 2 dây: SDA và SCL trong
đó SDA (serial data) là đường truyền nhận dữ liệu và SCL(serial clock) là đường
truyền xung nhịp. Các đường SDA và SCL trên các thiết bị có cấu hình cực máng hở
(open-drain) hoặc collector hở (open-collector) vì thế chúng cần điện trở kéo lên
(pull-up resistor) khoảng từ 1 đến 10(kΩ). Ở trạng thái nghỉ thì 2 chân này ở mức
cao. Lý do sử dụng một hệ thống cực máng hở (open drain) là để không xảy ra hiện
tượng ngắn mạch, điều này có thể xảy ra khi một thiết bị cố gắng kéo đường dây lên
cao và một số thiết bị khác cố gắng kéo đường dây xuống thấp.
Tốc độ truyền: 100Kbits/s ở chế độ chuẩn (Standard mode), có thể lên tới
400Kbits/s ở chế độ nhanh(Fast mode) và cao nhất là 3,4Mbits/s ở chế độ cao tốc
(High-speed mode).
Trên đường truyền giao tiếp I2C: Có thể là một chủ-một tớ (One Master One
Slave), một chủ- nhiều tớ (One Master-Multi Slave) hoặc nhiều chủ-nhiều tớ (Multi
Master –Multi Slave). Một thiết bị tớ Slave cũng có thể trở thành chip chủ nếu nó có
khả năng.
SVTH: Phan Phúc Hậu Trang 37
CHƯƠNG 2: ĐỐI TƯỢNG VÀ PHƯƠNG PHÁP NGHIÊM CỨU
Hình 2.19: Sơ đồ truyền dữ liệu I2C
2.2.8.2 Một số khái niệm quan trọng
Master: Giữ vai trò điều khiển bus I2C, tạo xung clock(SCL) trong suốt quá
trình giao tiếp, khởi động quá trình truyền nhận, phát đi địa chỉ của thiết bị cần giao
tiếp, gửi tín hiệu R/W tới Slave, truyền/nhận dữ liệu tới Slave, tạo tín hiệu NOT ACK
khi kết thúc nhận từ Slave, tạo tín hiệu kết thúc quá trình truy xuất.
Slave: Nhận địa chỉ và bit R/W từ Master (Chỉ “Response” khi đúng địa chỉ).
Nhận dữ liệu từ Master gửi bit ACK sau mỗi 8 Clock. Truyền dữ liệu tới Master, chờ
ACK từ Master để tiếp tục gửi.
Start condition (điều kiện bắt đầu): Đang ở trạng thái nghỉ (SDA=SCL=1),
Master muốn thực hiện một cuộc gọi thì nó sẽ kéo chân SDA xuống thấp trong khi
SCL vẫn ở mức cao. Khi điều kiện bắt đầu được gửi bởi thiết bị Master, tất cả các
thiết bị Slave đều hoạt động ngay cả khi chúng ở chế độ ngủ (sleep mode) và đợi bit
địa chỉ.
Stop condition (điều kiện kết thúc): Sau khi thực hiện truyền nhận dữ liệu nếu
Master muốn kết thúc thì nó sẽ kéo chân SDA lên cao trong khi SCL vẫn ở mức cao.
Điều kiện kết thúc chỉ được tạo ra sau khi địa chỉ hoặc dữ liệu đã được truyền nhận.
Hình 2.20: Điều kiện START và STOP
SVTH: Phan Phúc Hậu Trang 38
CHƯƠNG 2: ĐỐI TƯỢNG VÀ PHƯƠNG PHÁP NGHIÊM CỨU
Khoảng giữa Start condition và Stop condition là khoảng bận của đường truyền.
Các Master khác không thể tác động vào đường truyền trong khoảng thời gian này.
Sau khi kết thúc truyền nhận mà Master không gửi Stop condition mà lại gửi thêm
Start condition gọi là Repeat Start: xảy ra khi Master muốn lấy liên tiếp dữ liệu từ
Slave. Sau khi thiết bị truyền gửi byte dữ liệu thì sẽ có tín hiệu đáp trả (Response)
của thiết bị nhận. Tín hiệu đáp trả đã nhận được gọi là ACK (thiết bị nhận kéo SDA
xuống 0) và ngược lại là NACK(SDA vẫn ở mức cao).
Hình 2.21: Điều kiện REPEATED START
Mỗi thiết bị tham gia vào bus I2C đều có địa chỉ cố định có độ dài 7 bit như vậy
trên một bus I2C có thể phân biệt tối đa 128 thiết bị.
2.2.8.3 Thực hiện giao tiếp truyền nhận dữ liệu
Khi một Master muốn giao tiếp với 1 Slave nào đó trước hết nó tạo ra Start
Condition, tiếp theo là gửi địa chỉ của Slave cần giao tiếp (7 bit) + 1bit
READ/WRITE( mất 8 clock). Tại thời điểm clock thứ 9 (tức là xung trên SCL) nó sẽ
đọc xung gọi là ACK (tất nhiên là trên SDA) từ Slave. Xung ACK=0 khi Slave có
địa chỉ tương ứng đã response. Nếu xung ACK=1(NOT ACK) lúc này Master có
những xử lý phù hợp ví dụ có thể gửi Stop condition sau đó phát lại địa chỉ Slave
khác. Khi Slave có địa chỉ tương ứng đã respone tiếp đến Master sẽ truyền hoặc nhận
dữ liệu tùy thuộc vào bit READ/WRITE mà Master vừa gửi.
Hình 2.22: Giao thức truyền nhận dữ liệu I2C
SVTH: Phan Phúc Hậu Trang 39
CHƯƠNG 2: ĐỐI TƯỢNG VÀ PHƯƠNG PHÁP NGHIÊM CỨU
Nếu bit READ/WRITE=0 tức là Master muốn ghi dữ liệu vào Slave. Master bắt
đầu gửi dữ liệu đến Slave từng byte một (mất 8 clock/byte), clock thứ 9 Slave nhận
sẽ phát lại ACK(kéo SDA=0).
Nếu Slave phát tín hiệu NOT ACK (không tác động SDA ở xung thử 9)
(ACK=1) sau khi nhận dữ liệu thì Master sẽ kết thúc quá trình gửi bằng cách phát đi
Stop condition. Để kết thúc quá trình truyền khi mà Master truyền byte cuối nó cũng
sẽ tạo ra Stop condition.
Nếu bit READ/WRITE=1 tức là Master muốn đọc dữ liệu từ Slave thì Slave sẽ
gửi từng byte ra SDA. Master sẽ nhận từng byte dữ liệu và cũng trả về bit ACK =0
(kéo SDA xuống thấp ở clock 9) sau mỗi byte nhận. Nếu Master không muốn nhận
dữ liệu nữa nó sẽ gửi xung NOT‐ACK và tạo tín hiệu STOP.
Nếu Master gửi địa chỉ của Slave là 0 và bít READ/WRITE=0 thì tức là nó
muốn thực hiện một cuộc gọi chung tới tất cả các Slave. Slave đồng ý sẽ phát tín hiệu
ACK, không đồng ý sẽ phát tín hiệu NACK. Cuộc gọi chung thường xảy ra khi Master
muốn gửi dữ liệu chung đến các Slave. Cuộc gọi chung không có nghĩa khi phát địa
chỉ là 0 và đi sau là bit READ/WRITE=1 vì Master không thể nhận dữ liệu đồng thời
từ các Slave.
2.2.8.4 Đặc điểm truyền nhận
Dữ liệu được truyền trên bus I2C theo từng bit tại mỗi cạnh lên của xung (Clock
SCL) , sự thay đổi bit dữ liệu trên SDA xảy ra khi SCL đang ở mức thấp. Số lượng
byte có thể truyền trong một lần tùy ý tối đa là 128byte. Trái với UART, trong giao
thức I2C: bit MSB sẽ được truyền trước. Sau 8 xung clock 8 bit dữ liệu đã được truyền
đi, thiết bị nhận sẽ kéo SDA xuống mức thấp tương ứng một bit ACK tại xung clock
thứ 9 báo hiệu đã nhận đủ 8 bit. Thiết bị truyền khi nhận được bit ACK sẽ tiếp tục
thực hiện quá trình truyền hoặc kết thúc.
SVTH: Phan Phúc Hậu Trang 40
CHƯƠNG 3: TÍNH TOÁN VÀ THIẾT KẾ MÔ HÌNH
CHƯƠNG 3: THIẾT KẾ VÀ CHẾ TẠO MÔ HÌNH
3.1 Bài toán động học
3.1.1 Động học thuận
Bài toán động học thuận yêu cầu xác định vị trí và hướng của khâu tác động
cuối (end-effeector) khi biết trước các giá trị của biến liên kết.
Trong chuyển đổi Denavit-Hartenberg, ta đặt gốc tọa độ trùng với đế của Robot
thì ta có được bảng thông số DH như sau:
Bảng 3.1: Bảng thông số DH
Khâu ai 휶i di 휽i
*
1 a0 0 d1 0
*
2 a1 0 0 휃2
*
3 a2 0 0 휃3
Hình 3.1: Gắn hệ tọa độ cho Robot
Xác định các ma trận A:
- Ma trận An có dạng:
푐표푠휃푖 −푠푖푛휃푖푐표푠훼푖 푠푖푛휃푖푠푖푛훼푖 푎푖푐표푠휃푖
푠푖푛휃푖 푐표푠휃푖푐표푠훼푖 −푐표푠휃푖푠푖푛훼푖 푎푖푠푖푛휃푖
An = [ ] (3.1)
0 푠푖푛훼푖 푐표푠훼푖 푑푖
0 0 0 1
- Ma trận A1 mô tả quan hệ giữa khung tọa độ 1 với khung tọa độ gốc:
1 0 0 푎0
0 1 0 0
A1 = [ ]
0 0 1 푑1
0 0 0 1
- Ma trận A2 mô tả quan hệ giữa khung tọa độ 2 với khung tọa độ 1:
푐표푠휃2 −푠푖푛휃2 0 푎1푐표푠휃2
푠푖푛휃2 푐표푠휃2 0 푎1푠푖푛휃2
A2 = [ ]
0 0 1 0
0 0 0 1
SVTH: Phan Phúc Hậu Trang 41
CHƯƠNG 3: TÍNH TOÁN VÀ THIẾT KẾ MÔ HÌNH
- Ma trận A3 mô tả quan hệ giữa khung tọa độ 3 với khung tọa độ 2:
푐표푠휃3 −푠푖푛휃3 0 푎2푐표푠휃3
푠푖푛휃3 푐표푠휃3 0 푎2푠푖푛휃3
A3 = [ ]
0 0 1 0
0 0 0 1
- Ma trận A mô tả phương trình động học của Robot: (A = A1.A2.A3)
cos(휃2 + 휃3) −sin(휃2 + 휃3) 0 푎1푐표푠휃2 + 푎2cos(휃2 + 휃3) + 푎0
sin(휃 + 휃 ) cos(휃 + 휃 ) 0 푎 푠푖푛휃 + 푎 sin(휃 + 휃 )
A = [ 2 3 2 3 1 2 2 2 3 ] (3.2)
0 0 1 푑
1
0 0 0 1
3.1.2 Động học ngược
Bài toán động học ngược dùng để xác định giá trị các biến liên kết khi vị trí và
hướng của khâu tác động cuối được xác định trước. Để thuận tiện cho việc tính toán,
em lựa chọn phương pháp đại số để giải bài toán được trình bày cụ thể như sau:
- Theo như bài toán động học thuận thì ta có được vị trí của khâu tác động cuối
như sau:
xc = 푎1푐표푠휃2 + 푎2cos(휃2 + 휃3)
yc = 푎1푠푖푛휃2 + 푎2sin(휃2 + 휃3) (3.3)
zc = d1
- Ta có d1 = zc việc còn lại là tìm 휃2 và 휃3. Với 푥푐, 푦푐, 푎1, 푎2 đã biết thì bình
phương từng vế hai phương trình ta được:
2 2 2 2 2
푥푐 = 푎1 cos (휃2) + 2푎1푎2cos (휃2)cos(휃2 + 휃3) + 푎2 cos (휃2 + 휃3)
{ 2 2 2 2 2 (3.4)
푦푐 = 푎1 sin (휃2) + 2푎1푎2 sin(휃2) sin(휃2 + 휃3) + 푎2 sin (휃2 + 휃3)
- Cộng hai phương trình (3.4) với nhau và sau đó rút gọn ta được:
2 2 2 2
푥푐 + 푦푐 − 푎1 − 푎2 = 2푎1푎2[cos (휃2)cos (휃2 + 휃3) + sin(휃2) sin(휃2 + 휃3)] (3.5)
2 2 2 2
Tương đương: 푥푐 + 푦푐 − 푎1 − 푎2 = 2푎1푎2cos (휃3) (3.6)
2 2 2 2
푥푐 +푦푐 −푎1−푎2 2
Suy ra: cos (휃3) = ; sin(휃3) = ±√1 − cos (휃3)
2푎1푎2
⇒ 휃3 = 푎푡푎푛2(cos (휃3), sin(휃3)) (3.7)
- Thay lại 휃3 vào phương trình (3.3) ta có:
푥 = [푎 + 푎 푐표푠(휃 )]푐표푠 (휃 ) − 푎 푠푖푛 (휃 ) 푠푖푛( 휃 )
{ 푐 1 2 3 2 2 3 2 (3.8)
푦푐 = 푎2푠푖푛(휃3)푐표푠 (휃2) + [푎1 + 푎2푐표푠(휃3)] 푠푖푛( 휃2)
- Giải hệ phương trình (3.8) ta được:
SVTH: Phan Phúc Hậu Trang 42
CHƯƠNG 3: TÍNH TOÁN VÀ THIẾT KẾ MÔ HÌNH
푎1 + 푎2푐표푠(휃3) −푎2푠푖푛(휃3) 2 2 2 2
∆= | | = 푎1 + 푎2 + 2푎1푎2푐표푠 (휃3) = 푥푐 + 푦푐
푎2푠푖푛(휃3) 푎1 + 푎2푐표푠(휃3)
푥푐 −푎2푠푖푛(휃3)
∆1= | | = 푎1푥푐 + 푎2[푥푐푐표푠 (휃3) + 푦푐푠푖푛(휃3)]
푦푐 푎1 + 푎2푐표푠(휃3)
푎1 + 푎2푐표푠(휃3) 푥푐
∆2= | | = 푎1푦푐 + 푎2...duino
[11].
[12]. https://project.makerbox.vn/2018/05/02/chia-se-cach-dieu-khien-pid-thuat-toan-pid/
[13]. https://vutienblog.com/lap-trinh-Matlab-gui-lam-quen-giao-dien-gui-trong-Matlab/
[14]. https://www.mathworks.com/Matlabcentral/fileexchange/37611-color-detection
SVTH: Phan Phúc Hậu Trang 93
PHỤ LỤC
PHỤ LỤC
Code Master
#include
#include
#define SERVO_PIN 9
Servo gServo;
int servoAVal servoBVal servoCVal;
void setup() {
gServo.attach(SERVO_PIN);
Wire.begin();
Serial.begin(9600); //initialize serial comunication
}
void loop() {
while (Serial.available () > 2) { //Check if the serial data is available.
delay(20);
char serialRead = Serial.read ();
if (serialRead == 'a') {
servoAVal = Serial.parseInt ();
Serial.print(servoAVal);
int16_t bigNum = servoAVal;
byte myArray[2];
myArray[0] = (bigNum >> 8) &0xFF;
myArray[1] = bigNum &0xFF;
Wire.beginTransmission(2); // Transmit to device #8
Wire.write(myArray, 2);
Wire.endTransmission(); // Stop transmitting
}
if (serialRead == 'b') {
servoBVal = Serial.parseInt ();
Serial.print(servoBVal);
SVTH: Phan Phúc Hậu Trang 94
PHỤ LỤC
int16_t bigNum = servoBVal;
byte myArray[2];
myArray[0] = (bigNum >> 8) &0xFF;
myArray[1] = bigNum &0xFF;
Wire.beginTransmission(3); // Transmit to device #8
Wire.write(myArray, 2);
Wire.endTransmission(); // Stop transmitting
}
if (serialRead == 'c') {
servoCVal = Serial.parseInt ();
Serial.print(servoCVal);
int16_t bigNum = servoCVal;
byte myArray[2];
myArray[0] = (bigNum >> 8) &0xFF;
myArray[1] = bigNum &0xFF;
Wire.beginTransmission(4); // Transmit to device #8
Wire.write(myArray, 2);
Wire.endTransmission(); // Stop transmitting
}
if (serialRead == 'o') {
gServo.write(170);
}
if (serialRead == 'f') {
gServo.write(80);
}
}
}
Code Slave
#include
#include
SVTH: Phan Phúc Hậu Trang 95
PHỤ LỤC
#define MotEnable 6 //Motor Enamble pin Runs on PWM signal
#define MotFwd 4 // Motor Forward pin
#define MotRev 7 // Motor Reverse pin
int16_t bigNum;
int encoderPin1 = 2; //Encoder Output 'A' must connected with intreput pin of
Arduino.
int encoderPin2 = 3; //Encoder Otput 'B' must connected with intreput pin of
Arduino.
volatile int lastEncoded = 0; // Here updated value of encoder store.
volatile long encoderValue = 0; // Raw encoder value
int REV = 0; // Set point REQUIRED ENCODER VALUE
int lastMSB = 0; int lastLSB = 0;
double kp = 7.5 , ki = 0.18 , kd = 0.045 ; // Modify for optimal performance
double input = 0, output = 0, setpoint = 0;
PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT);
void setup() {
Wire.begin(3);
Wire.onReceive(receiveEvent);
pinMode(MotEnable, OUTPUT);
pinMode(MotFwd, OUTPUT);
pinMode(MotRev, OUTPUT);
Serial.begin(9600); //Initialize serial comunication
pinMode(encoderPin1, INPUT_PULLUP);
pinMode(encoderPin2, INPUT_PULLUP);
digitalWrite(encoderPin1, HIGH); //Turn pullup resistor on
digitalWrite(encoderPin2, HIGH); //Turn pullup resistor on
//Call updateEncoder() when any high/low changed seen
//on interrupt 0 (pin 2), or interrupt 1 (pin 3)
attachInterrupt(0, updateEncoder, CHANGE);
attachInterrupt(1, updateEncoder, CHANGE);
SVTH: Phan Phúc Hậu Trang 96
PHỤ LỤC
TCCR1B = TCCR1B &0b11111000 | 1;
myPID.SetMode(AUTOMATIC); //Set PID in Auto mode
myPID.SetSampleTime(1); // Refresh rate of PID controller
myPID.SetOutputLimits(-175, 175); // This is the MAX PWM value to move
motor, here change in value reflect change in speed of motor.
}
void loop(){
while (Wire.available()) {
byte a,b;
a = Wire.read();
b = Wire.read();
bigNum = a;
bigNum = bigNum << 8 | b;
}
REV = map (bigNum, 0, 360, 0, 9360); // Mapping degree into pulse
Serial.print("This is REV - ");
Serial.println(REV); // printing REV value
setpoint = REV; //PID while work to achive this value consider as SET
value
input = encoderValue ; // Data from encoder consider as a Process value
Serial.println("encoderValue- ");
Serial.println(encoderValue);
myPID.Compute(); // Calculate new output
pwmOut(output);
}
// Drive motor CW
void receiveEvent(int howMany) {}
void pwmOut(int out) {
if (out > 0) { // if REV > encoderValue motor move in forward direction.
analogWrite(MotEnable, out);
SVTH: Phan Phúc Hậu Trang 97
PHỤ LỤC
digitalWrite(MotFwd, HIGH);
digitalWrite(MotRev, LOW);
}
else { // if REV < encoderValue motor move in reverse direction.
analogWrite(MotEnable, abs(out));
digitalWrite(MotFwd, LOW);
digitalWrite(MotRev, HIGH);
}
}
void updateEncoder(){
int MSB = digitalRead(encoderPin1); //MSB = most significant bit
int LSB = digitalRead(encoderPin2); //LSB = least significant bit
int encoded = (MSB << 1) |LSB; //converting the 2 pin value to single number
int sum = (lastEncoded << 2) | encoded; //adding it to the previous encoded value
if(sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011)
encoderValue ++;
if(sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000)
encoderValue --;
lastEncoded = encoded; //store this value for next time
}
Code Matlab GUIDE
function varargout = GUIDE_LV(varargin)
%GUIDE_LV MATLAB code file for GUIDE_LV.fig
% GUIDE_LV, by itself, creates a new GUIDE_LV or raises the existing
% singleton*.
% H = GUIDE_LV returns the handle to a new GUIDE_LV or the handle to
% the existing singleton*.
% GUIDE_LV('Property','Value',...) creates a new GUIDE_LV using the
% given property value pairs. Unrecognized properties are passed via
% varargin to GUIDE_LV_OpeningFcn. This calling syntax produces a
SVTH: Phan Phúc Hậu Trang 98
PHỤ LỤC
% warning when there is an existing singleton*.
% GUIDE_LV('CALLBACK') and GUIDE_LV('CALLBACK',hObject,...) call
the
% local function named CALLBACK in GUIDE_LV.M with the given input
% arguments.
% *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one
% instance to run (singleton)".
% See also: GUIDE, GUIDATA, GUIHANDLES
% Edit the above text to modify the response to help GUIDE_LV
% Last Modified by GUIDE v2.5 25-Jun-2020 07:21:30
% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name', mfilename, ...
'gui_Singleton', gui_Singleton, ...
'gui_OpeningFcn', @GUIDE_LV_OpeningFcn, ...
'gui_OutputFcn', @GUIDE_LV_OutputFcn, ...
'gui_LayoutFcn', [], ...
'gui_Callback', []);
if nargin && ischar(varargin{1})
gui_State.gui_Callback = str2func(varargin{1});
end
if nargout
[varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT
% --- Executes just before GUIDE_LV is made visible.
function GUIDE_LV_OpeningFcn(hObject, eventdata, handles, varargin)
SVTH: Phan Phúc Hậu Trang 99
PHỤ LỤC
% This function has no output args, see OutputFcn.
% hObject handle to figure
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% varargin unrecognized PropertyName/PropertyValue pairs from the
% command line (see VARARGIN)
% Choose default command line output for GUIDE_LV
handles.output = hObject;
% Update handles structure
guidata(hObject, handles);
% UIWAIT makes GUIDE_LV wait for user response (see UIRESUME)
% uiwait(handles.figure1);
global s;
global connect;
global l1 l2 l3;
l1 = 200; l2 = 185; l3 = 180;
connect = 0;
if (strcmp(get(s,'Status'),'open'))
fclose(s);
end
set(handles.pumPorts,'Enable','on');
delete(s);
s = serial('COM1');
set(handles.pumPorts,'String',getAvailableComPort);
% --- Outputs from this function are returned to the command line.
function varargout = GUIDE_LV_OutputFcn(hObject, eventdata, handles)
% varargout cell array for returning output args (see VARARGOUT);
% hObject handle to figure
% eventdata reserved - to be defined in a future version of MATLAB
SVTH: Phan Phúc Hậu Trang 100
PHỤ LỤC
% handles structure with handles and user data (see GUIDATA)
% Get default command line output from handles structure
varargout{1} = handles.output;
% --- Executes during object deletion, before destroying properties.
function figure1_DeleteFcn(hObject, eventdata, handles)
% hObject handle to figure1 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
global s;
global myCam;
if (strcmp(get(s,'Status'),'open'))
fclose(s);
end
delete(s);
clear all;
% --- Executes on selection change in pumPorts.
function pumPorts_Callback(hObject, eventdata, handles)
% hObject handle to pumPorts (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: contents = cellstr(get(hObject,'String')) returns pumPorts contents as cell
array
% contents{get(hObject,'Value')} returns selected item from pumPorts
% --- Executes during object creation, after setting all properties.
function pumPorts_CreateFcn(hObject, eventdata, handles)
% hObject handle to pumPorts (see GCBO)
SVTH: Phan Phúc Hậu Trang 101
PHỤ LỤC
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
% Hint: popupmenu controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
% --- Executes during object creation, after setting all properties.
function figure1_CreateFcn(hObject, eventdata, handles)
% hObject handle to figure1 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
% --- Executes during object creation, after setting all properties.
% --- Executes on button press in Connect.
function Connect_Callback(hObject, eventdata, handles)
% hObject handle to Connect (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
global s;
global connect;
if connect == 0
connect = 1;
set(handles.Connect,'String','DISCONNECT');
set(handles.Connect,'BackgroundColor','[0 0 1]');
set(handles.pumPorts,'Enable','off');
portList = get(handles.pumPorts,'String');
portIndex = get(handles.pumPorts,'Value');
SVTH: Phan Phúc Hậu Trang 102
PHỤ LỤC
port = portList(portIndex,:);
set (s,'Port',char(port));
fopen (s);
else if connect == 1
connect = 0;
set(handles.Connect,'String','CONNECT');
set(handles.Connect,'BackgroundColor','[1 0 0]');
set(handles.pumPorts,'Enable','on');
try
fclose(s);
catch
portList = get(handles.pumPorts,'String');
portIndex = get(handles.pumPorts,'Value');
port = portList(portIndex,:);
fclose (s);
end end end
function d1_Callback(hObject, eventdata, handles)
% hObject handle to d1 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of d1 as text
% str2double(get(hObject,'String')) returns contents of d1 as a double
% --- Executes during object creation, after setting all properties.
function d1_CreateFcn(hObject, eventdata, handles)
% hObject handle to d1 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
SVTH: Phan Phúc Hậu Trang 103
PHỤ LỤC
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
function theta2_Callback(hObject, eventdata, handles)
% hObject handle to theta2 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of theta2 as text
% str2double(get(hObject,'String')) returns contents of theta2 as a double
% --- Executes during object creation, after setting all properties.
function theta2_CreateFcn(hObject, eventdata, handles)
% hObject handle to theta2 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
function theta3_Callback(hObject, eventdata, handles)
% hObject handle to theta3 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of theta3 as text
SVTH: Phan Phúc Hậu Trang 104
PHỤ LỤC
% str2double(get(hObject,'String')) returns contents of theta3 as a double
% --- Executes during object creation, after setting all properties.
function theta3_CreateFcn(hObject, eventdata, handles)
% hObject handle to theta3 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
function px_Callback(hObject, eventdata, handles)
% hObject handle to px (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of px as text
% str2double(get(hObject,'String')) returns contents of px as a double
% --- Executes during object creation, after setting all properties.
function px_CreateFcn(hObject, eventdata, handles)
% hObject handle to px (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
SVTH: Phan Phúc Hậu Trang 105
PHỤ LỤC
set(hObject,'BackgroundColor','white');
end
function py_Callback(hObject, eventdata, handles)
% hObject handle to py (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of py as text
% str2double(get(hObject,'String')) returns contents of py as a double
% --- Executes during object creation, after setting all properties.
function py_CreateFcn(hObject, eventdata, handles)
% hObject handle to py (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
function pz_Callback(hObject, eventdata, handles)
% hObject handle to pz (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of pz as text
% str2double(get(hObject,'String')) returns contents of pz as a double
% --- Executes during object creation, after setting all properties.
SVTH: Phan Phúc Hậu Trang 106
PHỤ LỤC
function pz_CreateFcn(hObject, eventdata, handles)
% hObject handle to pz (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
% --- Executes on button press in dht.
function dht_Callback(hObject, eventdata, handles)
% hObject handle to dht (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
global s;
global l1 l2 l3;
d1 = get(handles.d1,'String');
theta2 = get(handles.theta2,'String');
theta3 = get(handles.theta3,'String');
fprintf(s , 'ff');
fprintf(s , 'a'); fprintf(s , d1); pause(2)
fprintf(s , 'b'); fprintf(s , theta2);
fprintf(s , 'c'); fprintf(s , theta3);
d1 = str2num(d1);
theta2 = str2num(theta2);
theta3 = str2num(theta3);
T1 = [1 0 0 l3 ; 0 1 0 0 ; 0 0 1 d1 ; 0 0 0 1];
SVTH: Phan Phúc Hậu Trang 107
PHỤ LỤC
T2 = [cosd(theta2) -sind(theta2) 0 l1*cosd(theta2) ; sind(theta2) cosd(theta2) 0
l1*sind(theta2) ; 0 0 1 0 ; 0 0 0 1];
T3 = [cosd(theta3) -sind(theta3) 0 l2*cosd(theta3) ; sind(theta3) cosd(theta3) 0
l2*sind(theta3) ; 0 0 1 0 ; 0 0 0 1];
T = T1*T2*T3;
px = T(1,4); py = T(2,4); pz = T(3,4);
set(handles.px,'String',num2str(round(px,2)));
set(handles.py,'String',num2str(round(py,2)));
set(handles.pz,'String',num2str(round(pz,2)));
% --- Executes on button press in dhn.
function dhn_Callback(hObject, eventdata, handles)
% hObject handle to dhn (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
global s;
global l1 l2 l3;
px = get(handles.px,'String');
py = get(handles.py,'String');
pz = get(handles.pz,'String');
px = str2num(px)-l3;
py = str2num(py);
d1 = str2num(pz);
c3 = (px^2+py^2-l1^2-l2^2)/(2*l1*l2); s3 = sqrt(1-(c3)^2);
theta3 = atan2d(s3,c3);
denta = px^2+py^2;
denta1 = l1*px+l2*(px*c3+py*s3);
denta2 = l1*py+l2*(py*c3-px*s3);
c2 = denta1/denta; s2 = denta2/denta;
theta2 = atan2d(s2,c2);
SVTH: Phan Phúc Hậu Trang 108
PHỤ LỤC
fprintf(s , 'ff');
fprintf(s , 'a'); fprintf(s , num2str(d1)); pause(2)
fprintf(s , 'b'); fprintf(s , num2str(theta2));
fprintf(s , 'c'); fprintf(s , num2str(theta3));
set(handles.d1,'String',num2str(round(d1,2)));
set(handles.theta2,'String',num2str(round(theta2,2)));
set(handles.theta3,'String',num2str(round(theta3,2)));
% --- Executes on button press in reset.
function reset_Callback(hObject, eventdata, handles)
% hObject handle to reset (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
global s;
global l1 l2 l3;
fprintf(s , 'b'); fprintf(s , '0'); fprintf(s , 'c'); fprintf(s , '0'); pause(1)
fprintf(s , 'a'); fprintf(s , '0');
fprintf(s , 'oo');
set(handles.d1,'String',0);
set(handles.theta2,'String',0);
set(handles.theta3,'String',0);
set(handles.px,'String',l1+l2+l3);
set(handles.py,'String',0);
set(handles.pz,'String',0);
function axes1_CreateFcn(hObject, eventdata, handles)
% hObject handle to axes1 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
% Hint: place code in OpeningFcn to populate axes1
SVTH: Phan Phúc Hậu Trang 109
PHỤ LỤC
% --- Executes on button press in oncam.
function oncam_Callback(hObject, eventdata, handles)
% hObject handle to oncam (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
global myCam;
axes(handles.axes1);
myCam = videoinput('winvideo',1);
set(myCam,'ReturnedColorSpace','rgb');
triggerconfig(myCam,'manual');
set(myCam,'FramesPerTrigger',1 );
set(myCam,'TriggerRepeat', Inf);
myCam.FrameGrabInterval = 3;
hImage = image(zeros(576, 1024,3),'parent',handles.axes1);
preview(myCam,hImage);
% --- Executes on button press in offcam.
function offcam_Callback(hObject, eventdata, handles)
% hObject handle to offcam(see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
global myCam;
if (strcmp(get(myCam,'Running'),'on'))
flushdata(myCam);
stop(myCam);
else
imaqreset;
end
cla(handles.axes1,'reset');
SVTH: Phan Phúc Hậu Trang 110
PHỤ LỤC
% --- Executes on button press in setcam.
function setcam_Callback(hObject, eventdata, handles)
% hObject handle to setcam (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
global myCam;
global X Y;
axes(handles.axes1);
X = []; Y = []; i = 0;
rgb = getsnapshot(myCam);
im = imsubtract(rgb(:,:,1), rgb2gray(rgb));
im = medfilt2(im, [3 3]);
im = im2bw(im,0.15);
% Remove all those pixels less than 250px
im = bwareaopen(im,250);
% Label all the connected components in the image.
bw = bwlabel(im, 8);
% Here we do the image blob analysis.
% We get a set of properties for each labeled region.
L = regionprops(bw, 'all');
for n = 1:length(L)
W = L(n).BoundingBox(3);
H = L(n).BoundingBox(4);
S = L(n).Area;
if H/W > 0.5 && H/W < 2
if S >500 && S< 650
if S/(W*H)>0.5 && S/(W*H)<=1
i = i+1;
X(i) = L(n).Centroid(1);
SVTH: Phan Phúc Hậu Trang 111
PHỤ LỤC
Y(i) = L(n).Centroid(2);
end end end end
imshow(rgb);
hold on
delete(findall(gcf,'Type', 'Rectangle'));
rectangle('Position',[X(1) Y(1) X(length(X))-X(1) Y(length(Y))-
Y(1)],'EdgeColor','w','LineWidth',2);
rectangle('Position',[X(1)+(X(length(X))-X(1))/2-20 Y(1) 40 Y(length(Y))-
Y(1)],'EdgeColor','w','LineWidth',2);
hold off
--- Executes on button press in rgb.
function rgb_Callback(hObject, eventdata, handles)
% hObject handle to rgb (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
global myCam;
global s;
global l1 l2 l3;
global X Y ;
axes(handles.axes1);
cX = []; cY = []; Color = []; Shape = []; i = 0;
rgb = getsnapshot(myCam);
img = imcrop(rgb,[X(1) Y(1) X(length(X))-X(1) Y(length(Y))-Y(1)]);
img = imresize(img,[500 600]);
gray = rgb2gray(img);
gray = imadjust(gray);
bw = im2bw(gray,0.25);
% Remove all those pixels less than 250px
bw = bwareaopen(bw,250);
SVTH: Phan Phúc Hậu Trang 112
PHỤ LỤC
% Label all the connected components in the image.
label = bwlabel(bw, 8);
% Here we do the image blob analysis.
% We get a set of properties for each labeled region.
vat = regionprops(label, 'all');
imshow(img);
hold on;
for n = 1:length(vat)
W = vat(n).BoundingBox(3);
H = vat(n).BoundingBox(4);
S = vat(n).Area;
P = vat(n).Perimeter;
if H/W > 0.5 && H/W < 2
if S > 500 && S < 1500
if S/(W*H) > 0.5 && S/(W*H) <= 1
i = i+1;
S = vat(n).Area
cX(i) = vat(n).Centroid(1);
cY(i) = vat(n).Centroid(2);
imvat = imcrop(img,vat(n).BoundingBox);
T = (4*pi*S)/(P*P);
[c,r] = imfindcircles
(imvat,[9 21],'ObjectPolarity','bright','Sensitivity',0.9);
%RED
imRed = imsubtract(imvat(:,:,1), rgb2gray(imvat));
imRed = medfilt2(imRed, [3 3]);
imRed = im2bw(imRed,0.13);
WRed = sum (imRed (:));
BRed = sum (~ imRed (:));
%GREEN
SVTH: Phan Phúc Hậu Trang 113
PHỤ LỤC
imGreen = imsubtract(imvat(:,:,2), rgb2gray(imvat));
imGreen = medfilt2(imGreen, [3 3]);
imGreen = im2bw(imGreen,0.03);
WGreen = sum (imGreen (:));
BGreen = sum (~ imGreen (:));
%BLUE
imBlue = imsubtract(imvat(:,:,3), rgb2gray(imvat));
imBlue = medfilt2(imBlue, [3 3]);
imBlue = im2bw(imBlue,0.08);
WBlue = sum (imBlue (:));
BBlue = sum (~ imBlue (:));
if WRed > BRed
plot(cX(i),cY(i),'r+');
text(cX(i)+25, cY(i)-12, strcat('X: ', num2str(round(cY(i)))),'Color','r');
text(cX(i)+25, cY(i)+12, strcat('Y: ',
num2str(round(cX(i)-290))),'Color','r');
if (length(c) > 0) || (T > 0.9 && T < 1.1)
Color(i) = 'R';
Shape(i) = 'O';
viscircles([cX(i) cY(i)],r,'EdgeColor','r');
end
if ((T > 0.65 && T 0.7)) && (length(c) == 0)
Color(i) = 'R';
Shape(i) = 'X';
rectangle('Position',vat(n).BoundingBox,'EdgeColor','r','LineWidth',2);
end
end
if WGreen > BGreen
plot(cX(i),cY(i),'g+');
text(cX(i)+25, cY(i)-12, strcat('X: ',
SVTH: Phan Phúc Hậu Trang 114
PHỤ LỤC
num2str(round(cY(i)))),'Color','g');
text(cX(i)+25, cY(i)+12, strcat('Y: ',
num2str(round(cX(i)-290))),'Color','g');
if (length(c) > 0) || (T > 0.9 && T < 1.1)
Color(i) = 'G';
Shape(i) = 'O';
viscircles([cX(i) cY(i)],r,'EdgeColor','g');
end
if ((T > 0.65 && T 0.7)) && (length(c) == 0)
Color(i) = 'G';
Shape(i) = 'X';
rectangle('Position',vat(n).BoundingBox,'EdgeColor','g','LineWidth',2);
end
end
if WBlue > BBlue
plot(cX(i),cY(i),'b+');
text(cX(i)+25, cY(i)-12, strcat('X: ', num2str(round(cY(i)))),'Color','b');
text(cX(i)+25, cY(i)+12, strcat('Y: ',
num2str(round(cX(i)-290))),'Color','b');
if (length(c) > 0) || (T > 0.9 && T < 1.1)
Color(i) = 'B';
Shape(i) = 'O';
viscircles([cX(i) cY(i)],r,'EdgeColor','b');
end
if ((T > 0.65 && T 0.7)) && (length(c) == 0)
Color(i) = 'B';
Shape(i) = 'X';
rectangle('Position',vat(n).BoundingBox,'EdgeColor','b','LineWidth',2);
end end end end end end
hold off
SVTH: Phan Phúc Hậu Trang 115
PHỤ LỤC
fprintf(s , 'ff');
fprintf(s , 'a'); fprintf(s , '70'); pause(2)
for j = 1:i
px = cY(j)-l3; py = cX(j)-290; pz = 0; d1 = pz;
c3 = (px^2+py^2-l1^2-l2^2)/(2*l1*l2); s3 = sqrt(1-(c3)^2);
theta3 = atan2d(s3,c3);
denta = px^2+py^2;
denta1 = l1*px+l2*(px*c3+py*s3);
denta2 = l1*py+l2*(py*c3-px*s3);
c2 = denta1/denta; s2 = denta2/denta;
theta2 = atan2d(s2,c2);
set(handles.d1,'String',num2str(round(d1,2)));
set(handles.theta2,'String',num2str(round(theta2,2)));
set(handles.theta3,'String',num2str(round(theta3,2)));
set(handles.px,'String',num2str(round(cY(j),2)));
set(handles.py,'String',num2str(round(py,2)));
set(handles.pz,'String',num2str(round(pz,2)));
if Color(j) == 'R' && Shape(j) == 'O'
fprintf(s , 'b'); fprintf(s , num2str(theta2));
fprintf(s , 'c'); fprintf(s , num2str(theta3)); pause(3)
fprintf(s , 'a'); fprintf(s , '0'); pause(2)
fprintf(s , 'oo'); pause(1)
fprintf(s , 'a'); fprintf(s , '70'); pause(2)
fprintf(s , 'b'); fprintf(s , '40');
fprintf(s , 'c'); fprintf(s , '30'); pause(3)
fprintf(s , 'ff'); pause(1)
end
if Color(j) == 'G' && Shape(j) == 'O'
fprintf(s , 'b'); fprintf(s , num2str(theta2));
fprintf(s , 'c'); fprintf(s , num2str(theta3)); pause(3)
SVTH: Phan Phúc Hậu Trang 116
PHỤ LỤC
fprintf(s , 'a'); fprintf(s , '0'); pause(2)
fprintf(s , 'oo'); pause(1)
fprintf(s , 'a'); fprintf(s , '70'); pause(2)
fprintf(s , 'b'); fprintf(s , '37');
fprintf(s , 'c'); fprintf(s , '66'); pause(3)
fprintf(s , 'ff'); pause(1)
end
if Color(j) == 'B' && Shape(j) == 'O'
fprintf(s , 'b'); fprintf(s , num2str(theta2));
fprintf(s , 'c'); fprintf(s , num2str(theta3)); pause(3)
fprintf(s , 'a'); fprintf(s , '0'); pause(2)
fprintf(s , 'oo'); pause(1)
fprintf(s , 'a'); fprintf(s , '70'); pause(2)
fprintf(s , 'b'); fprintf(s , '49');
fprintf(s , 'c'); fprintf(s , '77'); pause(3)
fprintf(s , 'ff'); pause(1)
end
if Color(j) == 'R' && Shape(j) == 'X'
fprintf(s , 'b'); fprintf(s , num2str(theta2));
fprintf(s , 'c'); fprintf(s , num2str(theta3)); pause(3)
fprintf(s , 'a'); fprintf(s , '0'); pause(2)
fprintf(s , 'oo'); pause(1)
fprintf(s , 'a'); fprintf(s , '70'); pause(2)
fprintf(s , 'b'); fprintf(s , '-40');
fprintf(s , 'c'); fprintf(s , '-30'); pause(3)
fprintf(s , 'ff'); pause(1)
end
if Color(j) == 'G' && Shape(j) == 'X'
fprintf(s , 'b'); fprintf(s , num2str(theta2));
fprintf(s , 'c'); fprintf(s , num2str(theta3)); pause(3)
SVTH: Phan Phúc Hậu Trang 117
PHỤ LỤC
fprintf(s , 'a'); fprintf(s , '0'); pause(2)
fprintf(s , 'oo'); pause(1)
fprintf(s , 'a'); fprintf(s , '70'); pause(2)
fprintf(s , 'b'); fprintf(s , '-37');
fprintf(s , 'c'); fprintf(s , '-66'); pause(3)
fprintf(s , 'ff'); pause(1)
end
if Color(j) == 'B' && Shape(j) == 'X'
fprintf(s , 'b'); fprintf(s , num2str(theta2));
fprintf(s , 'c'); fprintf(s , num2str(theta3)); pause(3)
fprintf(s , 'a'); fprintf(s , '0'); pause(2)
fprintf(s , 'oo'); pause(1)
fprintf(s , 'a'); fprintf(s , '70'); pause(2)
fprintf(s , 'b'); fprintf(s , '-49');
fprintf(s , 'c'); fprintf(s , '-77'); pause(3)
fprintf(s , 'ff'); pause(1)
end
end
fprintf(s , 'b'); fprintf(s , '0');
fprintf(s , 'c'); fprintf(s , '0'); pause(3)
fprintf(s , 'a'); fprintf(s , '0');
fprintf(s , 'oo');
set(handles.d1,'String',0);
set(handles.theta2,'String',0);
set(handles.theta3,'String',0);
set(handles.px,'String',l1+l2+l3);
set(handles.py,'String',0);
set(handles.pz,'String',0);
SVTH: Phan Phúc Hậu Trang 118
Các file đính kèm theo tài liệu này:
- luan_van_thiet_ke_va_che_tao_mo_hinh_robot_scara_phan_loai_s.pdf