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

......................................................... ....................................................................................................................................... ....................................................................................................................................... ....................................................................................................................................... ....................................................................................................................................... ....................................................................................................................................... ....................................................................................................................................... ....................................................................................................................................... ....................................................................................................................................... ....................................................................................................................................... ....................................................................................................................................... ....................................................................................................................................... ....................................................................................................................................... ....................................................................................................................................... ....................................................................................................................................... ....................................................................................................................................... ....................................................................................................................................... ....................................................................................................................................... ....................................................................................................................................... ....................................................................................................................................... ....................................................................................................................................... ....................................................................................................................................... ....................................................................................................................................... ....................................................................................................................................... ....................................................................................................................................... ....................................................................................................................................... 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